YangOne/GrabBag
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

工件孔定位协议输出顺序同步更新了界面的顺序&暂存螺杆定位的工具盘的修改

Browse Source
This commit is contained in:
杰仔 2026-04-23 23:40:50 +08:00
parent c103a63519
commit 0331f3f319
14 changed files with 347 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h
View File

@ -35,9 +35,12 @@ struct HandEyeExtrinsic
double rotY = 0.0;
double rotZ = 0.0;
double approachOffset = 0.0;
double offsetX = 0.0;
double offsetY = 0.0;
double offsetZ = 0.0;
double outRotX = 0.0;
double outRotY = 0.0;
double outRotZ = 15.0;
double outRotZ = 14.56;
};
class DetectPresenter

135
App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp
View File

@ -396,6 +396,107 @@ CTRotationMatrix BuildRotationMatrix(const std::array<CTVec3D, 3>& axes)
return rotation;
}
// ============ ZXY 欧拉角分解/重建（绕开 ZYX 奇异区） ============
// ZXY 分解：R = Rz(α)·Rx(β)·Ry(γ)
// ZXY 奇异点在 β=±90°（R(2,1)=±1），与 ZYX 奇异点 pitch≈±90° 不重叠。
static void DecomposeRotationZXY(const CTRotationMatrix& R,
double& alpha, double& beta, double& gamma)
{
const double sb = std::max(-1.0, std::min(1.0, R.at(2, 1)));
const double betaRad = std::asin(sb);
const double cb = std::cos(betaRad);
if (std::fabs(cb) > 1e-7) {
alpha = std::atan2(-R.at(0, 1), R.at(1, 1)) * 180.0 / M_PI;
gamma = std::atan2(-R.at(2, 0), R.at(2, 2)) * 180.0 / M_PI;
} else {
alpha = std::atan2(R.at(1, 0), R.at(0, 0)) * 180.0 / M_PI;
gamma = 0.0;
}
beta = betaRad * 180.0 / M_PI;
}
static CTRotationMatrix RebuildRotationZXY(double alphaDeg, double betaDeg, double gammaDeg)
{
const double a = alphaDeg * M_PI / 180.0;
const double b = betaDeg * M_PI / 180.0;
const double g = gammaDeg * M_PI / 180.0;
const double ca = std::cos(a), sa = std::sin(a);
const double cb = std::cos(b), sb = std::sin(b);
const double cg = std::cos(g), sg = std::sin(g);
CTRotationMatrix R;
R.at(0,0) = ca*cg + sa*sb*sg; R.at(0,1) = -sa*cb; R.at(0,2) = ca*sg + sa*sb*cg;
R.at(1,0) = sa*cg - ca*sb*sg; R.at(1,1) = ca*cb; R.at(1,2) = sa*sg - ca*sb*cg;
R.at(2,0) = -cb*sg; R.at(2,1) = sb; R.at(2,2) = cb*cg;
return R;
}
// 直接 ZYX 分解：R = Rz(yaw)·Ry(pitch)·Rx(roll)
static void DecomposeRotationZYX(const CTRotationMatrix& R,
double& roll, double& pitch, double& yaw)
{
const double sp = std::max(-1.0, std::min(1.0, -R.at(2, 0)));
const double pitchRad = std::asin(sp);
const double cp = std::cos(pitchRad);
if (std::fabs(cp) > 1e-7) {
roll = std::atan2(R.at(2, 1), R.at(2, 2)) * 180.0 / M_PI;
yaw = std::atan2(R.at(1, 0), R.at(0, 0)) * 180.0 / M_PI;
} else {
yaw = std::atan2(-R.at(0, 1), R.at(1, 1)) * 180.0 / M_PI;
roll = 0.0;
}
pitch = pitchRad * 180.0 / M_PI;
}
// 稳定 ZYX 欧拉角提取：
// 1) |R(2,0)| ≤ threshold → 直接 ZYX 分解
// 2) |R(2,0)| > threshold → ZXY 分解（奇异点不重叠）→ 重建 R' → ZYX 分解
// 3) 用拍照法兰姿态做连续性修正（±360° 拉回参考附近）
static void ExtractStableZYXEulerDegrees(const CTRotationMatrix& R,
double& roll, double& pitch, double& yaw,
double refRoll, double refYaw)
{
constexpr double kSingularityThreshold = 0.9998;
const double absSinPitch = std::fabs(R.at(2, 0));
if (absSinPitch > kSingularityThreshold) {
LOG_INFO("[Algo Thread] ZYX singularity: |R(2,0)|=%.6f > %.4f\n",
absSinPitch, kSingularityThreshold);
// ZXY 分解
double alpha, beta, gamma;
DecomposeRotationZXY(R, alpha, beta, gamma);
LOG_INFO("[Algo Thread] ZXY: Z=%.3f, X=%.3f, Y=%.3f\n", alpha, beta, gamma);
// 重建 R' 并验证
CTRotationMatrix Rp = RebuildRotationZXY(alpha, beta, gamma);
double err = 0;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
err += (Rp.at(i, j) - R.at(i, j)) * (Rp.at(i, j) - R.at(i, j));
err = std::sqrt(err);
LOG_INFO("[Algo Thread] R' rebuild error: %.10f\n", err);
// 对 R' 做 ZYX 分解
DecomposeRotationZYX(Rp, roll, pitch, yaw);
LOG_INFO("[Algo Thread] ZYX from R': Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", roll, pitch, yaw);
} else {
DecomposeRotationZYX(R, roll, pitch, yaw);
}
// 连续性修正：拉到参考姿态附近
double yawDiff = yaw - refYaw;
while (yawDiff > 180.0) { yaw -= 360.0; yawDiff -= 360.0; }
while (yawDiff < -180.0) { yaw += 360.0; yawDiff += 360.0; }
double rollDiff = roll - refRoll;
while (rollDiff > 180.0) { roll -= 360.0; rollDiff -= 360.0; }
while (rollDiff < -180.0) { roll += 360.0; rollDiff += 360.0; }
}
}
DetectPresenter::DetectPresenter(/* args */)
@ -557,15 +658,18 @@ int DetectPresenter::DetectScrew(
const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes);
ScrewPosition pos;
pos.x = robotCenter.x;
pos.y = robotCenter.y;
pos.z = robotCenter.z;
pos.approachX = robotApproach.x;
pos.approachY = robotApproach.y;
pos.approachZ = robotApproach.z;
pos.x = robotCenter.x + extrinsic.offsetX;
pos.y = robotCenter.y + extrinsic.offsetY;
pos.z = robotCenter.z + extrinsic.offsetZ;
pos.approachX = robotApproach.x + extrinsic.offsetX;
pos.approachY = robotApproach.y + extrinsic.offsetY;
pos.approachZ = robotApproach.z + extrinsic.offsetZ;
#if 0
ExtractStableZYXEulerDegrees(rotation, pos.roll, pos.pitch, pos.yaw, rx, rz);
#else
RotationMatrixToConfiguredEulerDegrees(rotation, order, pos.roll, pos.pitch, pos.yaw);
// 拍照姿态作参考，把 gimbal lock / 多解区的解拉回拍照姿态附近，减少机械臂旋转量。
ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz);
#endif
detectionResult.positions.push_back(pos);
ScrewInfo info;
@ -666,17 +770,17 @@ int DetectPresenter::DetectToolDisk(
const CTVec3D eyeNormalDir = NormalizeVector(ToCTVec3D(poseInfo.normalDir));
ScrewPosition pos;
pos.x = robotCenter.x;
pos.y = robotCenter.y;
pos.z = robotCenter.z;
pos.x = robotCenter.x + extrinsic.offsetX;
pos.y = robotCenter.y + extrinsic.offsetY;
pos.z = robotCenter.z + extrinsic.offsetZ;
// 接近点：Eye 系沿工具盘 X 轴偏移 offset，再经 T 变换到 Robot 系；姿态与目标点共用。
const double offset = extrinsic.approachOffset;
const CTVec3D eyeCenter = ToCTVec3D(poseInfo.center);
const CTVec3D eyeApproach(eyeCenter.x + eyeXAxis.x * offset, eyeCenter.y + eyeXAxis.y * offset, eyeCenter.z + eyeXAxis.z * offset);
const CTVec3D robotApproach = eyeInHandTransform.transformPoint(eyeApproach);
pos.approachX = robotApproach.x;
pos.approachY = robotApproach.y;
pos.approachZ = robotApproach.z;
pos.approachX = robotApproach.x + extrinsic.offsetX;
pos.approachY = robotApproach.y + extrinsic.offsetY;
pos.approachZ = robotApproach.z + extrinsic.offsetZ;
const std::pair<double, double> approachEyeXY(eyeApproach.x, eyeApproach.y);
// 姿态：三轴直接取算法输出 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。
@ -700,9 +804,12 @@ int DetectPresenter::DetectToolDisk(
// 同时让最终输出欧拉角远离万向节锁；默认 0 不启用。
ApplyAxesRotation(robotAxes, extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ);
const CTRotationMatrix robotRotation = BuildRotationMatrix(robotAxes);
#if 0
ExtractStableZYXEulerDegrees(robotRotation, pos.roll, pos.pitch, pos.yaw, rx, rz);
#else
RotationMatrixToConfiguredEulerDegrees(robotRotation, order, pos.roll, pos.pitch, pos.yaw);
// 拍照姿态作参考，把 gimbal lock / 多解区的解拉回拍照姿态附近，减少机械臂旋转量。
ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz);
#endif
detectionResult.positions.push_back(pos);
if (debugParam.enableDebug && debugParam.printDetailLog) {

8
App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp
View File

@ -245,6 +245,9 @@ int ScrewPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResult
extrinsic.rotY = calibMatrix.rotY;
extrinsic.rotZ = calibMatrix.rotZ;
extrinsic.approachOffset = calibMatrix.approachOffset;
extrinsic.offsetX = calibMatrix.offsetX;
extrinsic.offsetY = calibMatrix.offsetY;
extrinsic.offsetZ = calibMatrix.offsetZ;
extrinsic.outRotX = calibMatrix.outRotX;
extrinsic.outRotY = calibMatrix.outRotY;
extrinsic.outRotZ = calibMatrix.outRotZ;
@ -252,11 +255,12 @@ int ScrewPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResult
}
}
LOG_INFO("[Algo Thread] camera=%d eulerOrder=%d poseOutputOrder=%d rot=(%.3f, %.3f, %.3f) outRot=(%.3f, %.3f, %.3f) approachOffset=%.3f\n",
LOG_INFO("[Algo Thread] camera=%d eulerOrder=%d poseOutputOrder=%d rot=(%.3f, %.3f, %.3f) outRot=(%.3f, %.3f, %.3f) approachOffset=%.3f offset=(%.3f, %.3f, %.3f)\n",
m_currentCameraIndex, extrinsic.eulerOrder, poseOutputOrder,
extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ,
extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ,
extrinsic.approachOffset);
extrinsic.approachOffset,
extrinsic.offsetX, extrinsic.offsetY, extrinsic.offsetZ);
DetectionResult detectionResult;
detectionResult.cameraIndex = m_currentCameraIndex;

14
App/ScrewPosition/ScrewPositionApp/dialogalgoarg.cpp
View File

@ -228,7 +228,10 @@ void DialogAlgoArg::loadHandEyeCalibConfig()
savedMatrix->rotX,
savedMatrix->rotY,
savedMatrix->rotZ,
savedMatrix->approachOffset);
savedMatrix->approachOffset,
savedMatrix->offsetX,
savedMatrix->offsetY,
savedMatrix->offsetZ);
} else {
const CalibMatrix calibMatrix = m_presenter->GetClibMatrix(info.cameraIndex - 1);
m_handEyeCalibWidget->setCalibData(info.cameraIndex,
@ -314,12 +317,19 @@ bool DialogAlgoArg::saveParams()
double rotY = calibMatrix.rotY;
double rotZ = calibMatrix.rotZ;
double approachOffset = calibMatrix.approachOffset;
if (m_handEyeCalibWidget->getExtrinsicData(camIdx, eulerOrder, rotX, rotY, rotZ, approachOffset)) {
double offsetX = calibMatrix.offsetX;
double offsetY = calibMatrix.offsetY;
double offsetZ = calibMatrix.offsetZ;
if (m_handEyeCalibWidget->getExtrinsicData(camIdx, eulerOrder, rotX, rotY, rotZ, approachOffset,
offsetX, offsetY, offsetZ)) {
calibMatrix.eulerOrder = eulerOrder;
calibMatrix.rotX = rotX;
calibMatrix.rotY = rotY;
calibMatrix.rotZ = rotZ;
calibMatrix.approachOffset = approachOffset;
calibMatrix.offsetX = offsetX;
calibMatrix.offsetY = offsetY;
calibMatrix.offsetZ = offsetZ;
}
newMatrixList.push_back(calibMatrix);

12
App/ScrewPosition/ScrewPositionApp/dialogalgoarg.ui
View File

@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>776</width>
<height>655</height>
<height>733</height>
</rect>
</property>
<property name="windowTitle">
@ -44,7 +44,7 @@
<x>40</x>
<y>80</y>
<width>701</width>
<height>501</height>
<height>571</height>
</rect>
</property>
<property name="font">
@ -779,7 +779,7 @@ QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px;
<property name="geometry">
<rect>
<x>40</x>
<y>590</y>
<y>670</y>
<width>120</width>
<height>45</height>
</rect>
@ -813,7 +813,7 @@ QPushButton:pressed {
<property name="geometry">
<rect>
<x>400</x>
<y>590</y>
<y>670</y>
<width>100</width>
<height>45</height>
</rect>
@ -847,7 +847,7 @@ QPushButton:pressed {
<property name="geometry">
<rect>
<x>520</x>
<y>590</y>
<y>670</y>
<width>100</width>
<height>45</height>
</rect>
@ -881,7 +881,7 @@ QPushButton:pressed {
<property name="geometry">
<rect>
<x>640</x>
<y>590</y>
<y>670</y>
<width>100</width>
<height>45</height>
</rect>

1
App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h
View File

@ -34,6 +34,7 @@ public:
const double clibMatrix[16],
int eulerOrder,
int dirVectorInvert,
int poseOutputOrder,
WorkpieceHoleDetectionResult& detectionResult);
};

42
App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp
View File

@ -31,6 +31,7 @@ int DetectPresenter::DetectWorkpieceHole(
const double clibMatrix[16],
int eulerOrder,
int dirVectorInvert,
int poseOutputOrder,
WorkpieceHoleDetectionResult& detectionResult)
{
if (laserLines.empty()) {
@ -185,13 +186,48 @@ int DetectPresenter::DetectWorkpieceHole(
SSG_EulerAngles robotRpy = rotationMatrixToEulerZYX(R_pose);
// 将机器人坐标系下的位姿添加到positions列表
// 根据 poseOutputOrder 重映射欧拉角输出顺序
HolePosition centerRobotPos;
centerRobotPos.x = ptRobot.x;
centerRobotPos.y = ptRobot.y;
centerRobotPos.z = ptRobot.z;
centerRobotPos.roll = robotRpy.roll;
centerRobotPos.pitch = robotRpy.pitch;
centerRobotPos.yaw = robotRpy.yaw;
switch (poseOutputOrder) {
case POSE_ORDER_RPY:
centerRobotPos.roll = robotRpy.roll;
centerRobotPos.pitch = robotRpy.pitch;
centerRobotPos.yaw = robotRpy.yaw;
break;
case POSE_ORDER_RYP:
centerRobotPos.roll = robotRpy.roll;
centerRobotPos.pitch = robotRpy.yaw;
centerRobotPos.yaw = robotRpy.pitch;
break;
case POSE_ORDER_PRY:
centerRobotPos.roll = robotRpy.pitch;
centerRobotPos.pitch = robotRpy.roll;
centerRobotPos.yaw = robotRpy.yaw;
break;
case POSE_ORDER_PYR:
centerRobotPos.roll = robotRpy.pitch;
centerRobotPos.pitch = robotRpy.yaw;
centerRobotPos.yaw = robotRpy.roll;
break;
case POSE_ORDER_YRP:
centerRobotPos.roll = robotRpy.yaw;
centerRobotPos.pitch = robotRpy.roll;
centerRobotPos.yaw = robotRpy.pitch;
break;
case POSE_ORDER_YPR:
centerRobotPos.roll = robotRpy.yaw;
centerRobotPos.pitch = robotRpy.pitch;
centerRobotPos.yaw = robotRpy.roll;
break;
default:
centerRobotPos.roll = robotRpy.roll;
centerRobotPos.pitch = robotRpy.pitch;
centerRobotPos.yaw = robotRpy.yaw;
break;
}
detectionResult.positions.push_back(centerRobotPos);

52
App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp
View File

@ -351,11 +351,16 @@ int WorkpieceHolePresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResult
int dirVectorInvert = configResult.plcRobotServerConfig.dirVectorInvert;
LOG_INFO("[Algo Thread] Using dir vector invert: %d\n", dirVectorInvert);
// 获取姿态输出顺序配置
int poseOutputOrder = configResult.plcRobotServerConfig.poseOutputOrder;
LOG_INFO("[Algo Thread] Using pose output order: %d\n", poseOutputOrder);
WorkpieceHoleDetectionResult detectionResult;
int nRet = m_pDetectPresenter->DetectWorkpieceHole(m_currentCameraIndex, detectionDataCache,
algorithmParams, debugParam, m_dataLoader,
currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, detectionResult);
currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert,
poseOutputOrder, detectionResult);
// 根据项目类型选择处理方式
if (GetStatusCallback<IYWorkpieceHoleStatus>()) {
QString err = QString("错误:%1").arg(nRet);
@ -836,7 +841,7 @@ void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectio
return;
}
// 获取姿态输出顺序配置
// 获取姿态输出顺序配置（用于日志）
ConfigResult configResult = m_pConfigManager->GetConfigResult();
int poseOutputOrder = configResult.plcRobotServerConfig.poseOutputOrder;
LOG_INFO("Using pose output order: %d\n", poseOutputOrder);
@ -846,48 +851,13 @@ void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectio
PLCModbusClient::CoordinateData coord;
// 使用 float 类型存储坐标数据
// detectionResult 中的欧拉角已在 DetectWorkpieceHole 中根据 poseOutputOrder 重映射
coord.x = static_cast<float>(pos.x);
coord.y = static_cast<float>(pos.y);
coord.z = static_cast<float>(pos.z);
// 根据姿态输出顺序配置调整输出
switch (poseOutputOrder) {
case POSE_ORDER_RPY: // Roll, Pitch, Yaw（默认）
coord.roll = static_cast<float>(pos.roll);
coord.pitch = static_cast<float>(pos.pitch);
coord.yaw = static_cast<float>(pos.yaw);
break;
case POSE_ORDER_RYP: // Roll, Yaw, Pitch
coord.roll = static_cast<float>(pos.roll);
coord.pitch = static_cast<float>(pos.yaw);
coord.yaw = static_cast<float>(pos.pitch);
break;
case POSE_ORDER_PRY: // Pitch, Roll, Yaw
coord.roll = static_cast<float>(pos.pitch);
coord.pitch = static_cast<float>(pos.roll);
coord.yaw = static_cast<float>(pos.yaw);
break;
case POSE_ORDER_PYR: // Pitch, Yaw, Roll
coord.roll = static_cast<float>(pos.pitch);
coord.pitch = static_cast<float>(pos.yaw);
coord.yaw = static_cast<float>(pos.roll);
break;
case POSE_ORDER_YRP: // Yaw, Roll, Pitch
coord.roll = static_cast<float>(pos.yaw);
coord.pitch = static_cast<float>(pos.roll);
coord.yaw = static_cast<float>(pos.pitch);
break;
case POSE_ORDER_YPR: // Yaw, Pitch, Roll
coord.roll = static_cast<float>(pos.yaw);
coord.pitch = static_cast<float>(pos.pitch);
coord.yaw = static_cast<float>(pos.roll);
break;
default: // 默认 RPY
coord.roll = static_cast<float>(pos.roll);
coord.pitch = static_cast<float>(pos.pitch);
coord.yaw = static_cast<float>(pos.yaw);
break;
}
coord.roll = static_cast<float>(pos.roll);
coord.pitch = static_cast<float>(pos.pitch);
coord.yaw = static_cast<float>(pos.yaw);
LOG_INFO("Workpiece: X=%.5f, Y=%.5f, Z=%.5f, R=%.5f, P=%.5f, Y=%.5f (order=%d)\n",
coord.x, coord.y, coord.z, coord.roll, coord.pitch, coord.yaw, poseOutputOrder);

2
App/WorkpieceHole/WorkpieceHoleApp/Version.h
View File

@ -6,7 +6,7 @@
// 版本字符串
#define WORKPIECEHOLE_VERSION_STRING "1.1.3"
#define WORKPIECEHOLE_BUILD_STRING "1"
#define WORKPIECEHOLE_BUILD_STRING "2"
#define WORKPIECEHOLE_FULL_VERSION_STRING "V" WORKPIECEHOLE_VERSION_STRING "_" WORKPIECEHOLE_BUILD_STRING
// 构建日期

3
App/WorkpieceHole/WorkpieceHoleApp/Version.md
View File

@ -1,4 +1,7 @@
# 1.1.3
## build_1 2026-04-21
1. 姿态角顺序同步控制页面显示
## build_1 2026-04-21
1. 算法更新

9
AppUtils/AppConfig/Inc/VrHandEyeCalibConfig.h
View File

@ -22,6 +22,10 @@ struct VrHandEyeCalibMatrix
double rotZ;
/* 接近点偏移 (mm): approach point offset along -axialDir (screw) / -normalDir (tool disk). */
double approachOffset;
/* 目标点在 Robot 坐标系下沿 X/Y/Z 轴的补偿偏移 (mm)。 */
double offsetX;
double offsetY;
double offsetZ;
double outRotX;
double outRotY;
double outRotZ;
@ -33,9 +37,12 @@ struct VrHandEyeCalibMatrix
, rotY(0.0)
, rotZ(0.0)
, approachOffset(0.0)
, offsetX(0.0)
, offsetY(0.0)
, offsetZ(0.0)
, outRotX(0.0)
, outRotY(0.0)
, outRotZ(15.0)
, outRotZ(14.56)
{
double identity[16] = {
1.0, 0.0, 0.0, 0.0,

9
AppUtils/AppConfig/Src/ConfigXmlUtils.cpp
View File

@ -38,6 +38,12 @@ static void LoadSingleHandEyeCalibMatrix(XMLElement* handEyeElement,
calibMatrix.rotZ = handEyeElement->DoubleAttribute("rotZ");
if (handEyeElement->Attribute("approachOffset"))
calibMatrix.approachOffset = handEyeElement->DoubleAttribute("approachOffset");
if (handEyeElement->Attribute("offsetX"))
calibMatrix.offsetX = handEyeElement->DoubleAttribute("offsetX");
if (handEyeElement->Attribute("offsetY"))
calibMatrix.offsetY = handEyeElement->DoubleAttribute("offsetY");
if (handEyeElement->Attribute("offsetZ"))
calibMatrix.offsetZ = handEyeElement->DoubleAttribute("offsetZ");
if (handEyeElement->Attribute("outRotX"))
calibMatrix.outRotX = handEyeElement->DoubleAttribute("outRotX");
if (handEyeElement->Attribute("outRotY"))
@ -108,6 +114,9 @@ void SaveHandEyeCalibMatrixs(XMLDocument& doc,
handEyeElement->SetAttribute("outRotY", calibMatrix.outRotY);
handEyeElement->SetAttribute("outRotZ", calibMatrix.outRotZ);
handEyeElement->SetAttribute("approachOffset", calibMatrix.approachOffset);
handEyeElement->SetAttribute("offsetX", calibMatrix.offsetX);
handEyeElement->SetAttribute("offsetY", calibMatrix.offsetY);
handEyeElement->SetAttribute("offsetZ", calibMatrix.offsetZ);
containerElement->InsertEndChild(handEyeElement);
}
root->InsertEndChild(containerElement);

26
AppUtils/UICommon/Inc/HandEyeCalibWidget.h
View File

@ -41,6 +41,9 @@ struct HandEyeCalibData
double rotY = 0.0;
double rotZ = 0.0;
double approachOffset = 0.0; // 接近点偏移（mm），沿目标姿态反方向远离目标点
double offsetX = 0.0; // 目标点补偿偏移 X（mm），Robot 坐标系
double offsetY = 0.0; // 目标点补偿偏移 Y（mm）
double offsetZ = 0.0; // 目标点补偿偏移 Z（mm）
};
/**
@ -83,6 +86,14 @@ public:
double rotX, double rotY, double rotZ,
double approachOffset);
/**
* @brief 设置指定相机的完整外参（含接近点偏移 + 目标点补偿偏移）
*/
void setExtrinsicData(int cameraIndex, int eulerOrder,
double rotX, double rotY, double rotZ,
double approachOffset,
double offsetX, double offsetY, double offsetZ);
/**
* @brief 获取指定相机的标定数据
* @return 是否存在该相机的数据
@ -104,6 +115,14 @@ public:
double& outRotX, double& outRotY, double& outRotZ,
double& outApproachOffset) const;
/**
* @brief 获取指定相机的完整外参（含接近点偏移 + 目标点补偿偏移）
*/
bool getExtrinsicData(int cameraIndex, int& outEulerOrder,
double& outRotX, double& outRotY, double& outRotZ,
double& outApproachOffset,
double& outOffsetX, double& outOffsetY, double& outOffsetZ) const;
/**
* @brief 获取当前选中的相机索引
* @return 相机索引，未选择返回 -1
@ -126,8 +145,9 @@ public:
void setExtrinsicControlsVisible(bool visible);
/**
* @brief 显示/隐藏接近点偏移控件（默认隐藏，仅需要 approach 点的 App 启用）
* @brief 显示/隐藏目标点补偿偏移控件（默认隐藏）
*/
void setTargetOffsetVisible(bool visible);
void setApproachOffsetVisible(bool visible);
signals:
@ -185,6 +205,10 @@ private:
QLineEdit* m_editRotZ;
QLabel* m_labelApproachOffset;
QLineEdit* m_editApproachOffset;
QLabel* m_labelTargetOffset;
QLineEdit* m_editOffsetX;
QLineEdit* m_editOffsetY;
QLineEdit* m_editOffsetZ;
QVector<HandEyeCalibData> m_calibDataCache; // 按相机索引缓存
QString m_defaultFilePath;

101
AppUtils/UICommon/Src/HandEyeCalibWidget.cpp
View File

@ -23,6 +23,10 @@ HandEyeCalibWidget::HandEyeCalibWidget(QWidget *parent)
, m_editRotZ(nullptr)
, m_labelApproachOffset(nullptr)
, m_editApproachOffset(nullptr)
, m_labelTargetOffset(nullptr)
, m_editOffsetX(nullptr)
, m_editOffsetY(nullptr)
, m_editOffsetZ(nullptr)
, m_matrixEditable(false)
{
memset(m_matrixEdits, 0, sizeof(m_matrixEdits));
@ -103,6 +107,26 @@ void HandEyeCalibWidget::setExtrinsicData(int cameraIndex, int eulerOrder,
}
}
void HandEyeCalibWidget::setExtrinsicData(int cameraIndex, int eulerOrder,
double rotX, double rotY, double rotZ,
double approachOffset,
double offsetX, double offsetY, double offsetZ)
{
HandEyeCalibData& data = ensureCalibData(cameraIndex);
data.eulerOrder = eulerOrder;
data.rotX = rotX;
data.rotY = rotY;
data.rotZ = rotZ;
data.approachOffset = approachOffset;
data.offsetX = offsetX;
data.offsetY = offsetY;
data.offsetZ = offsetZ;
if (currentCameraIndex() == cameraIndex) {
displayExtrinsic(data);
}
}
bool HandEyeCalibWidget::getCalibData(int cameraIndex, double outMatrix[16], bool& outIsCalibrated) const
{
// 如果当前显示的就是这个相机，且矩阵可编辑，优先从 UI 读取
@ -166,6 +190,31 @@ bool HandEyeCalibWidget::getExtrinsicData(int cameraIndex, int& outEulerOrder,
return true;
}
bool HandEyeCalibWidget::getExtrinsicData(int cameraIndex, int& outEulerOrder,
double& outRotX, double& outRotY, double& outRotZ,
double& outApproachOffset,
double& outOffsetX, double& outOffsetY, double& outOffsetZ) const
{
if (!getExtrinsicData(cameraIndex, outEulerOrder, outRotX, outRotY, outRotZ, outApproachOffset)) {
return false;
}
if (currentCameraIndex() == cameraIndex && m_editOffsetX && m_editOffsetY && m_editOffsetZ) {
outOffsetX = m_editOffsetX->text().trimmed().toDouble();
outOffsetY = m_editOffsetY->text().trimmed().toDouble();
outOffsetZ = m_editOffsetZ->text().trimmed().toDouble();
return true;
}
const HandEyeCalibData* data = findCalibData(cameraIndex);
if (data) {
outOffsetX = data->offsetX;
outOffsetY = data->offsetY;
outOffsetZ = data->offsetZ;
} else {
outOffsetX = outOffsetY = outOffsetZ = 0.0;
}
return true;
}
int HandEyeCalibWidget::currentCameraIndex() const
{
if (!m_comboCamera || m_comboCamera->count() == 0 || m_comboCamera->currentIndex() < 0) {
@ -208,6 +257,14 @@ void HandEyeCalibWidget::setApproachOffsetVisible(bool visible)
}
}
void HandEyeCalibWidget::setTargetOffsetVisible(bool visible)
{
if (m_labelTargetOffset) m_labelTargetOffset->setVisible(visible);
if (m_editOffsetX) m_editOffsetX->setVisible(visible);
if (m_editOffsetY) m_editOffsetY->setVisible(visible);
if (m_editOffsetZ) m_editOffsetZ->setVisible(visible);
}
// ========== 私有槽 ==========
void HandEyeCalibWidget::onCameraSelectionChanged(int index)
@ -522,6 +579,41 @@ void HandEyeCalibWidget::setupExtrinsicGroup()
m_labelApproachOffset->setVisible(false);
m_editApproachOffset->setVisible(false);
// 目标点补偿偏移 X/Y/Z
auto* validatorTargetOffset = new QDoubleValidator(-10000.0, 10000.0, 3, this);
validatorTargetOffset->setNotation(QDoubleValidator::StandardNotation);
m_editOffsetX = new QLineEdit(this);
m_editOffsetX->setFont(font);
m_editOffsetX->setStyleSheet(editStyle);
m_editOffsetX->setValidator(validatorTargetOffset);
m_editOffsetY = new QLineEdit(this);
m_editOffsetY->setFont(font);
m_editOffsetY->setStyleSheet(editStyle);
m_editOffsetY->setValidator(validatorTargetOffset);
m_editOffsetZ = new QLineEdit(this);
m_editOffsetZ->setFont(font);
m_editOffsetZ->setStyleSheet(editStyle);
m_editOffsetZ->setValidator(validatorTargetOffset);
QWidget* targetOffsetRow = new QWidget(this);
QHBoxLayout* targetOffsetLayout = new QHBoxLayout(targetOffsetRow);
targetOffsetLayout->setContentsMargins(0, 0, 0, 0);
targetOffsetLayout->setSpacing(8);
targetOffsetLayout->addWidget(makeAxisLabel(QString::fromUtf8("X:")));
targetOffsetLayout->addWidget(m_editOffsetX, 1);
targetOffsetLayout->addWidget(makeAxisLabel(QString::fromUtf8("Y:")));
targetOffsetLayout->addWidget(m_editOffsetY, 1);
targetOffsetLayout->addWidget(makeAxisLabel(QString::fromUtf8("Z:")));
targetOffsetLayout->addWidget(m_editOffsetZ, 1);
m_labelTargetOffset = new QLabel(QString::fromUtf8("目标点偏移 (mm):"), this);
m_labelTargetOffset->setFont(font);
m_labelTargetOffset->setStyleSheet(labelStyle);
form->addRow(m_labelTargetOffset, targetOffsetRow);
displayDefaultExtrinsic();
}
@ -598,6 +690,9 @@ void HandEyeCalibWidget::displayExtrinsic(const HandEyeCalibData& data)
if (m_editRotY) m_editRotY->setText(QString::number(data.rotY, 'f', 6));
if (m_editRotZ) m_editRotZ->setText(QString::number(data.rotZ, 'f', 6));
if (m_editApproachOffset) m_editApproachOffset->setText(QString::number(data.approachOffset, 'f', 3));
if (m_editOffsetX) m_editOffsetX->setText(QString::number(data.offsetX, 'f', 3));
if (m_editOffsetY) m_editOffsetY->setText(QString::number(data.offsetY, 'f', 3));
if (m_editOffsetZ) m_editOffsetZ->setText(QString::number(data.offsetZ, 'f', 3));
}
void HandEyeCalibWidget::displayDefaultExtrinsic()
@ -611,6 +706,9 @@ void HandEyeCalibWidget::displayDefaultExtrinsic()
if (m_editRotY) m_editRotY->setText(QString::number(0.0, 'f', 6));
if (m_editRotZ) m_editRotZ->setText(QString::number(0.0, 'f', 6));
if (m_editApproachOffset) m_editApproachOffset->setText(QString::number(0.0, 'f', 3));
if (m_editOffsetX) m_editOffsetX->setText(QString::number(0.0, 'f', 3));
if (m_editOffsetY) m_editOffsetY->setText(QString::number(0.0, 'f', 3));
if (m_editOffsetZ) m_editOffsetZ->setText(QString::number(0.0, 'f', 3));
}
bool HandEyeCalibWidget::readMatrixFromUI(double outMatrix[16]) const
@ -641,6 +739,9 @@ void HandEyeCalibWidget::commitExtrinsicToCache(int cameraIndex)
if (m_editApproachOffset) {
data.approachOffset = m_editApproachOffset->text().trimmed().toDouble();
}
if (m_editOffsetX) data.offsetX = m_editOffsetX->text().trimmed().toDouble();
if (m_editOffsetY) data.offsetY = m_editOffsetY->text().trimmed().toDouble();
if (m_editOffsetZ) data.offsetZ = m_editOffsetZ->text().trimmed().toDouble();
}
HandEyeCalibData* HandEyeCalibWidget::findCalibData(int cameraIndex)