682 lines
22 KiB
C++
682 lines
22 KiB
C++
#ifndef HANDEYECALIB_LIBRARY
|
||
#define HANDEYECALIB_LIBRARY
|
||
#endif
|
||
|
||
#include "HandEyeCalib.h"
|
||
#include "VrError.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::TransformPointWithCenter(
|
||
const HECRotationMatrix& R,
|
||
const HECTranslationVector& T,
|
||
const HECPoint3D& srcCenter,
|
||
const HECPoint3D& dstCenter,
|
||
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 srcCenter_eigen(srcCenter.x, srcCenter.y, srcCenter.z);
|
||
Eigen::Vector3d dstCenter_eigen(dstCenter.x, dstCenter.y, dstCenter.z);
|
||
Eigen::Vector3d src_eigen(srcPoint.x, srcPoint.y, srcPoint.z);
|
||
|
||
// 变换公式: dstPoint = R * (srcPoint - srcCenter) + dstCenter
|
||
Eigen::Vector3d dst_eigen = R_eigen * (src_eigen - srcCenter_eigen) + dstCenter_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::RotationMatrixToEulerZYX(
|
||
const HECRotationMatrix& R,
|
||
HECEulerAngles& angles)
|
||
{
|
||
RotationMatrixToEuler(R, HECEulerOrder::ZYX, angles);
|
||
}
|
||
|
||
void HandEyeCalib::RotationMatrixToEuler(
|
||
const HECRotationMatrix& R,
|
||
HECEulerOrder order,
|
||
HECEulerAngles& angles)
|
||
{
|
||
const double epsilon = 1e-6;
|
||
|
||
switch (order) {
|
||
case HECEulerOrder::ZYX: {
|
||
// R = Rz(yaw) * Ry(pitch) * Rx(roll)
|
||
// pitch = asin(-R[2,0])
|
||
angles.pitch = std::asin(-R.at(2, 0));
|
||
double cosPitch = std::cos(angles.pitch);
|
||
if (std::abs(cosPitch) > epsilon) {
|
||
angles.yaw = std::atan2(R.at(1, 0), R.at(0, 0));
|
||
angles.roll = std::atan2(R.at(2, 1), R.at(2, 2));
|
||
} else {
|
||
// 万向节锁
|
||
angles.roll = 0.0;
|
||
angles.yaw = std::atan2(-R.at(0, 1), R.at(1, 1));
|
||
}
|
||
break;
|
||
}
|
||
case HECEulerOrder::XYZ: {
|
||
// R = Rx(roll) * Ry(pitch) * Rz(yaw)
|
||
// pitch = asin(R[0,2])
|
||
angles.pitch = std::asin(R.at(0, 2));
|
||
double cosPitch = std::cos(angles.pitch);
|
||
if (std::abs(cosPitch) > epsilon) {
|
||
angles.roll = std::atan2(-R.at(1, 2), R.at(2, 2));
|
||
angles.yaw = std::atan2(-R.at(0, 1), R.at(0, 0));
|
||
} else {
|
||
angles.roll = std::atan2(R.at(2, 1), R.at(1, 1));
|
||
angles.yaw = 0.0;
|
||
}
|
||
break;
|
||
}
|
||
case HECEulerOrder::YZX: {
|
||
// R = Ry(pitch) * Rz(yaw) * Rx(roll)
|
||
// yaw = asin(R[1,0])
|
||
angles.yaw = std::asin(R.at(1, 0));
|
||
double cosYaw = std::cos(angles.yaw);
|
||
if (std::abs(cosYaw) > epsilon) {
|
||
angles.pitch = std::atan2(-R.at(2, 0), R.at(0, 0));
|
||
angles.roll = std::atan2(-R.at(1, 2), R.at(1, 1));
|
||
} else {
|
||
angles.pitch = std::atan2(R.at(0, 2), R.at(2, 2));
|
||
angles.roll = 0.0;
|
||
}
|
||
break;
|
||
}
|
||
case HECEulerOrder::YXZ: {
|
||
// R = Ry(pitch) * Rx(roll) * Rz(yaw)
|
||
// roll = asin(-R[1,2])
|
||
angles.roll = std::asin(-R.at(1, 2));
|
||
double cosRoll = std::cos(angles.roll);
|
||
if (std::abs(cosRoll) > epsilon) {
|
||
angles.pitch = std::atan2(R.at(0, 2), R.at(2, 2));
|
||
angles.yaw = std::atan2(R.at(1, 0), R.at(1, 1));
|
||
} else {
|
||
angles.pitch = std::atan2(-R.at(2, 0), R.at(0, 0));
|
||
angles.yaw = 0.0;
|
||
}
|
||
break;
|
||
}
|
||
case HECEulerOrder::XZY: {
|
||
// R = Rx(roll) * Rz(yaw) * Ry(pitch)
|
||
// yaw = asin(-R[0,1])
|
||
angles.yaw = std::asin(-R.at(0, 1));
|
||
double cosYaw = std::cos(angles.yaw);
|
||
if (std::abs(cosYaw) > epsilon) {
|
||
angles.roll = std::atan2(R.at(2, 1), R.at(1, 1));
|
||
angles.pitch = std::atan2(R.at(0, 2), R.at(0, 0));
|
||
} else {
|
||
angles.roll = std::atan2(-R.at(1, 2), R.at(2, 2));
|
||
angles.pitch = 0.0;
|
||
}
|
||
break;
|
||
}
|
||
case HECEulerOrder::ZXY: {
|
||
// R = Rz(yaw) * Rx(roll) * Ry(pitch)
|
||
// roll = asin(R[2,1])
|
||
angles.roll = std::asin(R.at(2, 1));
|
||
double cosRoll = std::cos(angles.roll);
|
||
if (std::abs(cosRoll) > epsilon) {
|
||
angles.yaw = std::atan2(-R.at(0, 1), R.at(1, 1));
|
||
angles.pitch = std::atan2(-R.at(2, 0), R.at(2, 2));
|
||
} else {
|
||
angles.yaw = std::atan2(R.at(1, 0), R.at(0, 0));
|
||
angles.pitch = 0.0;
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
void HandEyeCalib::EulerZYXToRotationMatrix(
|
||
const HECEulerAngles& angles,
|
||
HECRotationMatrix& R)
|
||
{
|
||
EulerToRotationMatrix(angles, HECEulerOrder::ZYX, R);
|
||
}
|
||
|
||
void HandEyeCalib::EulerToRotationMatrix(
|
||
const HECEulerAngles& angles,
|
||
HECEulerOrder order,
|
||
HECRotationMatrix& R)
|
||
{
|
||
double cr = std::cos(angles.roll);
|
||
double sr = std::sin(angles.roll);
|
||
double cp = std::cos(angles.pitch);
|
||
double sp = std::sin(angles.pitch);
|
||
double cy = std::cos(angles.yaw);
|
||
double sy = std::sin(angles.yaw);
|
||
|
||
switch (order) {
|
||
case HECEulerOrder::ZYX: {
|
||
// R = Rz(yaw) * Ry(pitch) * Rx(roll)
|
||
R.at(0, 0) = cy * cp;
|
||
R.at(0, 1) = cy * sp * sr - sy * cr;
|
||
R.at(0, 2) = cy * sp * cr + sy * sr;
|
||
R.at(1, 0) = sy * cp;
|
||
R.at(1, 1) = sy * sp * sr + cy * cr;
|
||
R.at(1, 2) = sy * sp * cr - cy * sr;
|
||
R.at(2, 0) = -sp;
|
||
R.at(2, 1) = cp * sr;
|
||
R.at(2, 2) = cp * cr;
|
||
break;
|
||
}
|
||
case HECEulerOrder::XYZ: {
|
||
// R = Rx(roll) * Ry(pitch) * Rz(yaw)
|
||
R.at(0, 0) = cp * cy;
|
||
R.at(0, 1) = -cp * sy;
|
||
R.at(0, 2) = sp;
|
||
R.at(1, 0) = sr * sp * cy + cr * sy;
|
||
R.at(1, 1) = -sr * sp * sy + cr * cy;
|
||
R.at(1, 2) = -sr * cp;
|
||
R.at(2, 0) = -cr * sp * cy + sr * sy;
|
||
R.at(2, 1) = cr * sp * sy + sr * cy;
|
||
R.at(2, 2) = cr * cp;
|
||
break;
|
||
}
|
||
case HECEulerOrder::YZX: {
|
||
// R = Ry(pitch) * Rz(yaw) * Rx(roll)
|
||
R.at(0, 0) = cp * cy;
|
||
R.at(0, 1) = sr * sp - cr * cp * sy;
|
||
R.at(0, 2) = cr * sp + sr * cp * sy;
|
||
R.at(1, 0) = sy;
|
||
R.at(1, 1) = cr * cy;
|
||
R.at(1, 2) = -sr * cy;
|
||
R.at(2, 0) = -sp * cy;
|
||
R.at(2, 1) = sr * cp + cr * sp * sy;
|
||
R.at(2, 2) = cr * cp - sr * sp * sy;
|
||
break;
|
||
}
|
||
case HECEulerOrder::YXZ: {
|
||
// R = Ry(pitch) * Rx(roll) * Rz(yaw)
|
||
R.at(0, 0) = cp * cy + sp * sr * sy;
|
||
R.at(0, 1) = -cp * sy + sp * sr * cy;
|
||
R.at(0, 2) = sp * cr;
|
||
R.at(1, 0) = cr * sy;
|
||
R.at(1, 1) = cr * cy;
|
||
R.at(1, 2) = -sr;
|
||
R.at(2, 0) = -sp * cy + cp * sr * sy;
|
||
R.at(2, 1) = sp * sy + cp * sr * cy;
|
||
R.at(2, 2) = cp * cr;
|
||
break;
|
||
}
|
||
case HECEulerOrder::XZY: {
|
||
// R = Rx(roll) * Rz(yaw) * Ry(pitch)
|
||
R.at(0, 0) = cp * cy;
|
||
R.at(0, 1) = -sy;
|
||
R.at(0, 2) = sp * cy;
|
||
R.at(1, 0) = cr * cp * sy + sr * sp;
|
||
R.at(1, 1) = cr * cy;
|
||
R.at(1, 2) = cr * sp * sy - sr * cp;
|
||
R.at(2, 0) = sr * cp * sy - cr * sp;
|
||
R.at(2, 1) = sr * cy;
|
||
R.at(2, 2) = sr * sp * sy + cr * cp;
|
||
break;
|
||
}
|
||
case HECEulerOrder::ZXY: {
|
||
// R = Rz(yaw) * Rx(roll) * Ry(pitch)
|
||
R.at(0, 0) = cy * cp - sy * sr * sp;
|
||
R.at(0, 1) = -sy * cr;
|
||
R.at(0, 2) = cy * sp + sy * sr * cp;
|
||
R.at(1, 0) = sy * cp + cy * sr * sp;
|
||
R.at(1, 1) = cy * cr;
|
||
R.at(1, 2) = sy * sp - cy * sr * cp;
|
||
R.at(2, 0) = -cr * sp;
|
||
R.at(2, 1) = sr;
|
||
R.at(2, 2) = cr * cp;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
void HandEyeCalib::TransformPose(
|
||
const HECCalibResult& calibResult,
|
||
const HECPoint3D& eyePoint,
|
||
const std::vector<HECPoint3D>& eyeDirVectors,
|
||
bool invertYZ,
|
||
HECPoseResult& poseResult)
|
||
{
|
||
// 1. 位置转换: P_robot = R * P_eye + T
|
||
TransformPoint(calibResult.R, calibResult.T, eyePoint, poseResult.position);
|
||
|
||
// 2. 姿态转换
|
||
if (eyeDirVectors.size() < 3) {
|
||
// 如果没有方向向量，姿态设为0
|
||
poseResult.angles = HECEulerAngles();
|
||
return;
|
||
}
|
||
|
||
// 复制方向向量
|
||
std::vector<HECPoint3D> dirVectors = eyeDirVectors;
|
||
|
||
// 如果需要，对Y轴和Z轴方向取反（坐标系方向调整）
|
||
if (invertYZ) {
|
||
dirVectors[1] = dirVectors[1] * (-1.0);
|
||
dirVectors[2] = dirVectors[2] * (-1.0);
|
||
}
|
||
|
||
// 对每个方向向量应用旋转矩阵
|
||
std::vector<HECPoint3D> robotDirVectors(3);
|
||
for (int i = 0; i < 3; i++) {
|
||
RotatePoint(calibResult.R, dirVectors[i], robotDirVectors[i]);
|
||
}
|
||
|
||
// 构建旋转矩阵（列向量形式）
|
||
HECRotationMatrix R_pose;
|
||
R_pose.at(0, 0) = robotDirVectors[0].x;
|
||
R_pose.at(0, 1) = robotDirVectors[1].x;
|
||
R_pose.at(0, 2) = robotDirVectors[2].x;
|
||
R_pose.at(1, 0) = robotDirVectors[0].y;
|
||
R_pose.at(1, 1) = robotDirVectors[1].y;
|
||
R_pose.at(1, 2) = robotDirVectors[2].y;
|
||
R_pose.at(2, 0) = robotDirVectors[0].z;
|
||
R_pose.at(2, 1) = robotDirVectors[1].z;
|
||
R_pose.at(2, 2) = robotDirVectors[2].z;
|
||
|
||
// 从旋转矩阵提取欧拉角
|
||
RotationMatrixToEulerZYX(R_pose, poseResult.angles);
|
||
}
|
||
|
||
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;
|
||
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];
|
||
totalError += diff.norm();
|
||
}
|
||
|
||
return totalError / N;
|
||
}
|
||
|
||
int HandEyeCalib::CalculateEyeInHand(
|
||
const std::vector<HECEyeInHandData>& calibData,
|
||
HECCalibResult& result)
|
||
{
|
||
int N = static_cast<int>(calibData.size());
|
||
if (N < 2) {
|
||
return ERR_CODE(APP_ERR_PARAM);
|
||
}
|
||
|
||
// 眼在手上标定使用 AX=XB 问题的求解方法
|
||
// A = T_end_i^{-1} * T_end_j (两次末端位姿之间的相对变换)
|
||
// B = T_cam (相机观测到的标定点相对变换)
|
||
// X = T_cam_to_end (待求的相机到末端的变换)
|
||
|
||
// 构建方程组，使用最小二乘法求解
|
||
// 这里使用简化方法：假设标定点固定，通过多组数据求解
|
||
|
||
// 收集所有相机坐标系下的点，转换到基座坐标系
|
||
// P_base = T_end * T_cam * P_cam
|
||
// 对于固定标定点，所有 P_base 应该相同
|
||
|
||
// 使用迭代优化方法求解
|
||
// 初始估计：使用第一组数据
|
||
Eigen::Matrix4d T_cam = Eigen::Matrix4d::Identity();
|
||
|
||
// 构建超定方程组
|
||
// 对于每对数据 (i, j)，有：
|
||
// T_end_i * T_cam * P_cam_i = T_end_j * T_cam * P_cam_j
|
||
// 即 T_end_i^{-1} * T_end_j * T_cam * P_cam_j = T_cam * P_cam_i
|
||
|
||
// 使用 Tsai-Lenz 方法求解旋转部分
|
||
std::vector<Eigen::Matrix3d> A_rot, B_rot;
|
||
std::vector<Eigen::Vector3d> A_trans, B_trans;
|
||
|
||
for (int i = 0; i < N - 1; i++) {
|
||
// 获取末端位姿
|
||
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[i + 1].endPose.at(r, c);
|
||
}
|
||
}
|
||
|
||
// A = T_end_i^{-1} * T_end_j
|
||
Eigen::Matrix4d A = T_end_i.inverse() * T_end_j;
|
||
|
||
// 相机观测点
|
||
Eigen::Vector3d P_cam_i(calibData[i].targetInCamera.x,
|
||
calibData[i].targetInCamera.y,
|
||
calibData[i].targetInCamera.z);
|
||
Eigen::Vector3d P_cam_j(calibData[i + 1].targetInCamera.x,
|
||
calibData[i + 1].targetInCamera.y,
|
||
calibData[i + 1].targetInCamera.z);
|
||
|
||
A_rot.push_back(A.block<3, 3>(0, 0));
|
||
A_trans.push_back(A.block<3, 1>(0, 3));
|
||
|
||
// B 矩阵从相机观测构建（假设标定点固定）
|
||
// 这里简化处理，直接使用点的差异
|
||
B_rot.push_back(Eigen::Matrix3d::Identity());
|
||
B_trans.push_back(P_cam_i - P_cam_j);
|
||
}
|
||
|
||
// 使用 SVD 求解旋转矩阵
|
||
// 构建 M 矩阵用于求解旋转
|
||
Eigen::MatrixXd M(9 * (N - 1), 9);
|
||
M.setZero();
|
||
|
||
for (int i = 0; i < N - 1; i++) {
|
||
// (A_rot ⊗ I - I ⊗ B_rot^T) * vec(X_rot) = 0
|
||
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++) {
|
||
// Kronecker product 展开
|
||
int row = i * 9 + r * 3 + c;
|
||
for (int k = 0; k < 3; k++) {
|
||
M(row, r * 3 + k) += Ai(c, k);
|
||
M(row, k * 3 + c) -= Bi(r, k);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 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;
|
||
}
|
||
|
||
// 求解平移向量
|
||
// 使用最小二乘法: (A_rot - I) * t_cam = R_cam * B_trans - A_trans
|
||
Eigen::MatrixXd C(3 * (N - 1), 3);
|
||
Eigen::VectorXd d(3 * (N - 1));
|
||
|
||
for (int i = 0; i < N - 1; 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;
|
||
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> basePoints;
|
||
for (int i = 0; i < N; i++) {
|
||
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);
|
||
}
|
||
}
|
||
|
||
Eigen::Vector4d P_cam(calibData[i].targetInCamera.x,
|
||
calibData[i].targetInCamera.y,
|
||
calibData[i].targetInCamera.z, 1.0);
|
||
Eigen::Vector4d P_base = T_end * T_result * P_cam;
|
||
basePoints.push_back(P_base.head<3>());
|
||
}
|
||
|
||
// 计算基座点的离散程度作为误差
|
||
Eigen::Vector3d meanBase = Eigen::Vector3d::Zero();
|
||
for (const auto& p : basePoints) {
|
||
meanBase += p;
|
||
}
|
||
meanBase /= N;
|
||
|
||
for (const auto& p : basePoints) {
|
||
totalError += (p - meanBase).norm();
|
||
}
|
||
result.error = totalError / N;
|
||
|
||
result.centerEye = HECPoint3D(0, 0, 0);
|
||
result.centerRobot = HECPoint3D(meanBase(0), meanBase(1), meanBase(2));
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
int HandEyeCalib::CalculateEyeInHandWithTarget(
|
||
const std::vector<HECEyeInHandData>& calibData,
|
||
const HECPoint3D& targetInBase,
|
||
HECCalibResult& result)
|
||
{
|
||
int N = static_cast<int>(calibData.size());
|
||
if (N < 3) {
|
||
return ERR_CODE(APP_ERR_PARAM);
|
||
}
|
||
|
||
// 当标定点在基座坐标系下的位置已知时
|
||
// P_base = T_end * T_cam * P_cam
|
||
// T_cam = T_end^{-1} * T_base_to_cam
|
||
// 其中 T_base_to_cam 将 P_cam 变换到 P_base
|
||
|
||
// 收集相机坐标系下的点和对应的末端逆变换后的基座点
|
||
std::vector<HECPoint3D> camPoints;
|
||
std::vector<HECPoint3D> endPoints;
|
||
|
||
Eigen::Vector3d P_base(targetInBase.x, targetInBase.y, targetInBase.z);
|
||
|
||
for (int i = 0; i < N; i++) {
|
||
// 获取末端位姿的逆
|
||
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);
|
||
}
|
||
}
|
||
Eigen::Matrix4d T_end_inv = T_end.inverse();
|
||
|
||
// 将基座点变换到末端坐标系
|
||
Eigen::Vector4d P_base_h(P_base(0), P_base(1), P_base(2), 1.0);
|
||
Eigen::Vector4d P_end = T_end_inv * P_base_h;
|
||
|
||
camPoints.push_back(calibData[i].targetInCamera);
|
||
endPoints.push_back(HECPoint3D(P_end(0), P_end(1), P_end(2)));
|
||
}
|
||
|
||
// 使用 SVD 方法求解相机到末端的变换
|
||
// 这与 EyeToHand 的 CalculateRT 方法相同
|
||
return CalculateRT(camPoints, endPoints, result);
|
||
}
|
||
|
||
// 导出函数实现
|
||
IHandEyeCalib* CreateHandEyeCalibInstance()
|
||
{
|
||
return new HandEyeCalib();
|
||
}
|
||
|
||
void DestroyHandEyeCalibInstance(IHandEyeCalib* instance)
|
||
{
|
||
delete instance;
|
||
}
|