From f0de4a15da9bac521093175e004a5fd920b85054 Mon Sep 17 00:00:00 2001 From: yiyi Date: Sun, 29 Mar 2026 10:48:35 +0800 Subject: [PATCH] eye to robot --- .../Presenter/Inc/DetectPresenter.h | 2 + .../Presenter/Src/DetectPresenter.cpp | 228 +++++++++--- .../Src/RodAndBarPositionPresenter.cpp | 12 +- .../RodAndBarPositionApp.pro | 18 +- .../RodAndBarPositionApp/dialogalgoarg.cpp | 22 ++ Tools/CalibView/Inc/CalibViewMainWindow.h | 16 + Tools/CalibView/Src/CalibDataWidget.cpp | 98 +++--- Tools/CalibView/Src/CalibViewMainWindow.cpp | 325 +++++++++++++++++- 8 files changed, 609 insertions(+), 112 deletions(-) diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h index 3a63b44..11855de 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h @@ -33,6 +33,8 @@ public: const VrDebugParam& debugParam, LaserDataLoader& dataLoader, const double clibMatrix[16], + int eulerOrder, + int dirVectorInvert, DetectionResult& detectionResult); }; diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp index cb17db1..14ec744 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp @@ -1,13 +1,143 @@ #include "DetectPresenter.h" #include "rodAndBarDetection_Export.h" #include "AlgoParamConverter.h" +#include "IHandEyeCalib.h" #include #define _USE_MATH_DEFINES #include +#include #include #include #include +namespace +{ + +constexpr double kVectorNormEpsilon = 1e-6; + +HECPoint3D MakeHecPoint(double x, double y, double z) +{ + return HECPoint3D(x, y, z); +} + +HECPoint3D CrossProduct(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); +} + +double DotProduct(const HECPoint3D& a, const HECPoint3D& b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +bool NormalizeInPlace(HECPoint3D& v) +{ + const double norm = v.norm(); + if (norm < kVectorNormEpsilon) { + return false; + } + + v = v / norm; + return true; +} + +void ApplyDirVectorInvert(std::vector& dirVectors, int dirVectorInvert) +{ + 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; + case 0: + default: + break; + } +} + +bool BuildRightHandedFrameFromXZ(const HECPoint3D& xSeed, + const HECPoint3D& zSeed, + std::vector& dirVectors) +{ + HECPoint3D xAxis = xSeed; + HECPoint3D zAxis = zSeed; + if (!NormalizeInPlace(xAxis) || !NormalizeInPlace(zAxis)) { + return false; + } + + zAxis = zAxis - xAxis * DotProduct(xAxis, zAxis); + if (!NormalizeInPlace(zAxis)) { + return false; + } + + HECPoint3D yAxis = CrossProduct(zAxis, xAxis); + if (!NormalizeInPlace(yAxis)) { + return false; + } + + zAxis = CrossProduct(xAxis, yAxis); + if (!NormalizeInPlace(zAxis)) { + return false; + } + + dirVectors = { xAxis, yAxis, zAxis }; + return true; +} + +HECRotationMatrix BuildRotationMatrixFromAxes(const std::vector& dirVectors) +{ + HECRotationMatrix rotation; + rotation.at(0, 0) = dirVectors[0].x; + rotation.at(0, 1) = dirVectors[1].x; + rotation.at(0, 2) = dirVectors[2].x; + rotation.at(1, 0) = dirVectors[0].y; + rotation.at(1, 1) = dirVectors[1].y; + rotation.at(1, 2) = dirVectors[2].y; + rotation.at(2, 0) = dirVectors[0].z; + rotation.at(2, 1) = dirVectors[1].z; + rotation.at(2, 2) = dirVectors[2].z; + return rotation; +} + +HECEulerOrder ToHandEyeEulerOrder(int eulerOrder) +{ + switch (eulerOrder) { + case 10: return HECEulerOrder::XYZ; + case 11: return HECEulerOrder::ZYX; + case 12: return HECEulerOrder::ZXY; + case 13: return HECEulerOrder::YXZ; + case 14: return HECEulerOrder::YZX; + case 15: return HECEulerOrder::XZY; + default: + LOG_WARNING("Unsupported euler order %d, fallback to 11(sZYX)\n", eulerOrder); + return HECEulerOrder::ZYX; + } +} + +HECCalibResult ToHandEyeCalibResult(const double clibMatrix[16]) +{ + HECCalibResult calibResult; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + calibResult.R.at(i, j) = clibMatrix[i * 4 + j]; + } + calibResult.T.at(i) = clibMatrix[i * 4 + 3]; + } + return calibResult; +} + +} // namespace + DetectPresenter::DetectPresenter(/* args */) { LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_rodAndBarDetectionVersion()); @@ -29,6 +159,8 @@ int DetectPresenter::DetectRod( const VrDebugParam& debugParam, LaserDataLoader& dataLoader, const double clibMatrix[16], + int eulerOrder, + int dirVectorInvert, DetectionResult& detectionResult) { if (laserLines.empty()) { @@ -98,9 +230,18 @@ int DetectPresenter::DetectRod( LOG_DEBUG("after sx_rodPositioning \n"); - LOG_INFO("sx_rodPositioning: detected %zu rods, err=%d runtime=%.3fms\n", - rodInfo.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec()); + LOG_INFO("sx_rodPositioning: detected %zu rods, err=%d runtime=%.3fms\n", rodInfo.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec()); ERR_CODE_RETURN(errCode); + std::unique_ptr handEyeCalib( + CreateHandEyeCalibInstance(), + DestroyHandEyeCalibInstance); + if (!handEyeCalib) { + LOG_ERROR("Failed to create HandEyeCalib instance\n"); + return ERR_CODE(DEV_NOT_FIND); + } + + const HECCalibResult calibResult = ToHandEyeCalibResult(clibMatrix); + const HECEulerOrder hecEulerOrder = ToHandEyeEulerOrder(eulerOrder); // 构建检测结果：生成点云图像 // 1. 获取所有棒材的中心点用于可视化 @@ -215,44 +356,55 @@ int DetectPresenter::DetectRod( targetObj.y = rod.center.y; targetObj.z = rod.center.z; - SVzNL3DPoint robotObj; - CVrConvert::EyeToRobot(targetObj, robotObj, clibMatrix); + HECPoint3D eyePoint = MakeHecPoint(targetObj.x, targetObj.y, targetObj.z); + HECPoint3D robotPoint; + handEyeCalib->TransformPoint(calibResult.R, calibResult.T, eyePoint, robotPoint); + + std::vector dirVectorsEye; + bool validPose = BuildRightHandedFrameFromXZ( + MakeHecPoint(rod.axialDir.x, rod.axialDir.y, rod.axialDir.z), + MakeHecPoint(rod.normalDir.x, rod.normalDir.y, rod.normalDir.z), + dirVectorsEye); + if (!validPose) { + LOG_WARNING("[Algo Thread] Rod %zu has invalid axial/normal direction, use zero pose\n", i); + dirVectorsEye = { + HECPoint3D(1.0, 0.0, 0.0), + HECPoint3D(0.0, 1.0, 0.0), + HECPoint3D(0.0, 0.0, 1.0) + }; + } + ApplyDirVectorInvert(dirVectorsEye, dirVectorInvert); + + std::vector dirVectorsRobot(3); + for (int axisIdx = 0; axisIdx < 3; ++axisIdx) { + handEyeCalib->RotatePoint(calibResult.R, dirVectorsEye[axisIdx], dirVectorsRobot[axisIdx]); + } + + const HECRotationMatrix robotPoseR = BuildRotationMatrixFromAxes(dirVectorsRobot); + + HECEulerAngles robotEuler; + handEyeCalib->RotationMatrixToEuler(robotPoseR, hecEulerOrder, robotEuler); + double rollDeg = 0.0; + double pitchDeg = 0.0; + double yawDeg = 0.0; + robotEuler.toDegrees(rollDeg, pitchDeg, yawDeg); // 创建位置数据（使用转换后的机械臂坐标） RodPosition pos; - pos.x = robotObj.x; // 机械臂坐标X - pos.y = robotObj.y; // 机械臂坐标Y - pos.z = robotObj.z; // 机械臂坐标Z - // 从轴向方向和法向方向构建旋转矩阵，提取欧拉角 (ZYX外旋) - // X轴 = axialDir, Z轴 = normalDir, Y轴 = cross(Z, X) - double ax = rod.axialDir.x, ay = rod.axialDir.y, az = rod.axialDir.z; - double nx = rod.normalDir.x, ny = rod.normalDir.y, nz = rod.normalDir.z; - // Y = cross(normal, axial) - double yx = ny * az - nz * ay; - double yy = nz * ax - nx * az; - double yz = nx * ay - ny * ax; - // 旋转矩阵 R = [axialDir | Y | normalDir] (列向量) - // R[0][0]=ax, R[1][0]=ay, R[2][0]=az (第一列: X轴=axialDir) - // R[0][1]=yx, R[1][1]=yy, R[2][1]=yz (第二列: Y轴) - // R[0][2]=nx, R[1][2]=ny, R[2][2]=nz (第三列: Z轴=normalDir) - // ZYX外旋欧拉角提取: - // pitch = -asin(R[2][0]) - // roll = atan2(R[2][1], R[2][2]) - // yaw = atan2(R[1][0], R[0][0]) - double sinPitch = -az; // R[2][0] = az - if (sinPitch > 1.0) sinPitch = 1.0; - if (sinPitch < -1.0) sinPitch = -1.0; - pos.pitch = asin(sinPitch) * 180.0 / M_PI; - pos.roll = atan2(yz, nz) * 180.0 / M_PI; // atan2(R[2][1], R[2][2]) - pos.yaw = atan2(ay, ax) * 180.0 / M_PI; // atan2(R[1][0], R[0][0]) + pos.roll = rollDeg; + pos.pitch = pitchDeg; + pos.yaw = yawDeg; + pos.x = robotPoint.x; // 机械臂坐标X + pos.y = robotPoint.y; // 机械臂坐标Y + pos.z = robotPoint.z; // 机械臂坐标Z detectionResult.positions.push_back(pos); // 保存棒材信息 RodInfo info; - info.centerX = robotObj.x; - info.centerY = robotObj.y; - info.centerZ = robotObj.z; + info.centerX = robotPoint.x; + info.centerY = robotPoint.y; + info.centerZ = robotPoint.z; info.axialDirX = rod.axialDir.x; info.axialDirY = rod.axialDir.y; info.axialDirZ = rod.axialDir.z; @@ -268,14 +420,10 @@ int DetectPresenter::DetectRod( detectionResult.rodInfoList.push_back(info); if(debugParam.enableDebug && debugParam.printDetailLog){ - LOG_INFO("[Algo Thread] Rod %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", - i, rod.center.x, rod.center.y, rod.center.z); - LOG_INFO("[Algo Thread] Rod %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, RPY=%.2f, %.2f, %.2f\n", - i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw); - LOG_INFO("[Algo Thread] Rod %zu Axial Dir: X=%.3f, Y=%.3f, Z=%.3f\n", - i, rod.axialDir.x, rod.axialDir.y, rod.axialDir.z); - LOG_INFO("[Algo Thread] Rod %zu Normal Dir: X=%.3f, Y=%.3f, Z=%.3f\n", - i, rod.normalDir.x, rod.normalDir.y, rod.normalDir.z); + LOG_INFO("[Algo Thread] Rod %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", i, rod.center.x, rod.center.y, rod.center.z); + LOG_INFO("[Algo Thread] Rod %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, RPY=%.2f, %.2f, %.2f\n", i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw); + LOG_INFO("[Algo Thread] Rod %zu Axial Dir: X=%.3f, Y=%.3f, Z=%.3f\n", i, rod.axialDir.x, rod.axialDir.y, rod.axialDir.z); + LOG_INFO("[Algo Thread] Rod %zu Normal Dir: X=%.3f, Y=%.3f, Z=%.3f\n", i, rod.normalDir.x, rod.normalDir.y, rod.normalDir.z); } } diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp index cb9ee2c..db73f81 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp @@ -207,12 +207,22 @@ int RodAndBarPositionPresenter::ProcessAlgoDetection(std::vectorGetAlgorithmParams(); ConfigResult configResult = m_pConfigManager->GetConfigResult(); VrDebugParam debugParam = configResult.debugParam; + int eulerOrder = configResult.eulerOrder; + const int calibIdx = m_currentCameraIndex - 1; + if (calibIdx >= 0 && calibIdx < static_cast(configResult.handEyeCalibMatrixList.size())) { + eulerOrder = configResult.handEyeCalibMatrixList[calibIdx].eulerOrder; + } else if (!configResult.handEyeCalibMatrixList.empty()) { + eulerOrder = configResult.handEyeCalibMatrixList[0].eulerOrder; + } + const int dirVectorInvert = configResult.dirVectorInvert; + LOG_INFO("[Algo Thread] Using euler order: %d, dir vector invert: %d\n", eulerOrder, dirVectorInvert); DetectionResult detectionResult; int nRet = m_pDetectPresenter->DetectRod(m_currentCameraIndex, detectionDataCache, algorithmParams, debugParam, m_dataLoader, - currentClibMatrix.clibMatrix, detectionResult); + currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, + detectionResult); // 根据项目类型选择处理方式 if (GetStatusCallback()) { QString err = QString("错误:%1").arg(nRet); diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/RodAndBarPositionApp.pro b/App/RodAndBarPosition/RodAndBarPositionApp/RodAndBarPositionApp.pro index 0c8d248..57f2fb7 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/RodAndBarPositionApp.pro +++ b/App/RodAndBarPosition/RodAndBarPositionApp/RodAndBarPositionApp.pro @@ -37,6 +37,7 @@ INCLUDEPATH += $$PWD/../../../AppUtils/UICommon/Inc INCLUDEPATH += $$PWD/../../../AppUtils/AppCommon/Inc INCLUDEPATH += $$PWD/../../../AppUtils/AppConfig/Inc INCLUDEPATH += $$PWD/../../../Module/AuthModule/Inc +INCLUDEPATH += $$PWD/../../../Module/HandEyeCalib/Inc # 源文件 SOURCES += \ @@ -79,6 +80,7 @@ win32:CONFIG(debug, debug|release) { LIBS += -L../../../AppUtils/AppConfig/debug -lAppConfig LIBS += -L../../../AppUtils/AppCommon/debug -lAppCommon LIBS += -L../../../Module/AuthModule/debug -lAuthModule + LIBS += -L../../../Module/HandEyeCalib/debug -lHandEyeCalib LIBS += -L../RodAndBarPositionConfig/debug -lRodAndBarPositionConfig LIBS += -L../../../Device/VrEyeDevice/debug -lVrEyeDevice LIBS += -L../../../Module/ModbusTCPServer/debug -lModbusTCPServer @@ -92,6 +94,7 @@ win32:CONFIG(debug, debug|release) { LIBS += -L../../../AppUtils/AppConfig/release -lAppConfig LIBS += -L../../../AppUtils/AppCommon/release -lAppCommon LIBS += -L../../../Module/AuthModule/release -lAuthModule + LIBS += -L../../../Module/HandEyeCalib/release -lHandEyeCalib LIBS += -L../RodAndBarPositionConfig/release -lRodAndBarPositionConfig LIBS += -L../../../Device/VrEyeDevice/release -lVrEyeDevice LIBS += -L../../../Module/ModbusTCPServer/release -lModbusTCPServer @@ -100,13 +103,14 @@ win32:CONFIG(debug, debug|release) { }else:unix:!macx { # Unix/Linux平台库链接（包括交叉编译） # 注意链接顺序：依赖关系从高到低（被依赖的库放在后面） - LIBS += -L../RodAndBarPositionConfig -lRodAndBarPositionConfig - LIBS += -L../../../AppUtils/UICommon -lUICommon - LIBS += -L../../../AppUtils/AppCommon -lAppCommon - LIBS += -L../../../AppUtils/AppConfig -lAppConfig - LIBS += -L../../../Module/AuthModule -lAuthModule - LIBS += -L../../../Utils/CloudUtils -lCloudUtils - LIBS += -L../../../Device/VrEyeDevice -lVrEyeDevice + LIBS += -L../RodAndBarPositionConfig -lRodAndBarPositionConfig + LIBS += -L../../../AppUtils/UICommon -lUICommon + LIBS += -L../../../AppUtils/AppCommon -lAppCommon + LIBS += -L../../../AppUtils/AppConfig -lAppConfig + LIBS += -L../../../Module/AuthModule -lAuthModule + LIBS += -L../../../Module/HandEyeCalib -lHandEyeCalib + LIBS += -L../../../Utils/CloudUtils -lCloudUtils + LIBS += -L../../../Device/VrEyeDevice -lVrEyeDevice LIBS += -L../../../Module/ModbusTCPServer -lModbusTCPServer LIBS += -L../../../VrNets -lVrModbus LIBS += -L../../../Module/ShareMem -lShareMem diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp b/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp index 69c1b84..d0e9475 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp +++ b/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp @@ -107,6 +107,21 @@ void DialogAlgoArg::saveParams() if (m_presenter->GetConfigManager()) { ConfigManager* configManager = m_presenter->GetConfigManager(); SystemConfig systemConfig = configManager->GetConfig(); + NetworkConfigData netConfig; + bool hasNetworkConfig = false; + + if (m_networkConfigWidget) { + netConfig = m_networkConfigWidget->getConfig(); + hasNetworkConfig = true; + systemConfig.configResult.eulerOrder = netConfig.eulerOrder; + systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert; + systemConfig.configResult.byteOrder = netConfig.byteOrder; + } + if (hasNetworkConfig) { + for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) { + calibMatrix.eulerOrder = netConfig.eulerOrder; + } + } // 保存手眼标定矩阵 if (m_handEyeCalibWidget) { @@ -128,6 +143,7 @@ void DialogAlgoArg::saveParams() break; } } + calibMatrix.eulerOrder = systemConfig.configResult.eulerOrder; // 如果控件中有更新的数据则覆盖 bool isCalibrated = false; @@ -299,6 +315,9 @@ void DialogAlgoArg::loadNetworkConfig() NetworkConfigData netConfig; netConfig.eulerOrder = configResult.eulerOrder; + if (!configResult.handEyeCalibMatrixList.empty()) { + netConfig.eulerOrder = configResult.handEyeCalibMatrixList[0].eulerOrder; + } netConfig.dirVectorInvert = configResult.dirVectorInvert; netConfig.byteOrder = configResult.byteOrder; @@ -319,6 +338,9 @@ void DialogAlgoArg::saveNetworkConfig() systemConfig.configResult.eulerOrder = netConfig.eulerOrder; systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert; systemConfig.configResult.byteOrder = netConfig.byteOrder; + for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) { + calibMatrix.eulerOrder = netConfig.eulerOrder; + } // 更新并保存 configManager->UpdateFullConfig(systemConfig); diff --git a/Tools/CalibView/Inc/CalibViewMainWindow.h b/Tools/CalibView/Inc/CalibViewMainWindow.h index 257a719..c2e9638 100644 --- a/Tools/CalibView/Inc/CalibViewMainWindow.h +++ b/Tools/CalibView/Inc/CalibViewMainWindow.h @@ -59,6 +59,8 @@ private slots: * @brief 执行坐标变换测试 */ void onTransformTest(); + void onEulerToDirectionMatrix(); + void onDirectionMatrixToEuler(); /** * @brief 执行 TCP 标定 @@ -142,6 +144,9 @@ private: * @brief 追加日志 */ void appendLog(const QString& message); + void updateDirectionMatrixDisplay(const HECRotationMatrix& R); + bool tryGetDirectionMatrixInput(HECRotationMatrix& R); + QString eulerOrderToString(HECEulerOrder order) const; // 标定实例 IHandEyeCalib* m_calib; @@ -162,8 +167,19 @@ private: QDoubleSpinBox* m_sbRoll; QDoubleSpinBox* m_sbPitch; QDoubleSpinBox* m_sbYaw; + QDoubleSpinBox* m_sbDirXX; + QDoubleSpinBox* m_sbDirXY; + QDoubleSpinBox* m_sbDirXZ; + QDoubleSpinBox* m_sbDirYX; + QDoubleSpinBox* m_sbDirYY; + QDoubleSpinBox* m_sbDirYZ; + QDoubleSpinBox* m_sbDirZX; + QDoubleSpinBox* m_sbDirZY; + QDoubleSpinBox* m_sbDirZZ; QComboBox* m_cbAngleUnit; QComboBox* m_cbEulerOrder; + QPushButton* m_btnEulerToMatrix; + QPushButton* m_btnMatrixToEuler; QPushButton* m_btnTransform; // 右侧面板 - 日志 diff --git a/Tools/CalibView/Src/CalibDataWidget.cpp b/Tools/CalibView/Src/CalibDataWidget.cpp index d721baf..e122cab 100644 --- a/Tools/CalibView/Src/CalibDataWidget.cpp +++ b/Tools/CalibView/Src/CalibDataWidget.cpp @@ -480,8 +480,8 @@ void CalibDataWidget::getEyeToHandPoseData(std::vector& calibD double boardZ = m_tableHandEye->item(row, 2) ? m_tableHandEye->item(row, 2)->text().toDouble() : 0; - // 获取标定板在相机坐标系的姿态（Rodrigues旋转向量） - // 表格中存储的值根据角度单位可能是弧度或角度 + // 获取标定板在相机坐标系的姿态 + // 表格中存储的是欧拉角，当前单位可能是弧度或角度 double boardRoll = m_tableHandEye->item(row, 3) ? m_tableHandEye->item(row, 3)->text().toDouble() : 0; double boardPitch = m_tableHandEye->item(row, 4) ? @@ -496,38 +496,10 @@ void CalibDataWidget::getEyeToHandPoseData(std::vector& calibD boardYaw = boardYaw * M_PI / 180.0; } - // 标定板姿态是Rodrigues旋转向量，需要转换为旋转矩阵 - // 旋转向量的模长是旋转角度，方向是旋转轴 - double theta = std::sqrt(boardRoll*boardRoll + boardPitch*boardPitch + boardYaw*boardYaw); + // 标定板姿态与机器人姿态统一使用当前选中的欧拉角顺序 + HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw); HECRotationMatrix R_board; - - if (theta < 1e-6) { - // 旋转角度接近0，使用单位矩阵 - R_board.at(0, 0) = 1; R_board.at(0, 1) = 0; R_board.at(0, 2) = 0; - R_board.at(1, 0) = 0; R_board.at(1, 1) = 1; R_board.at(1, 2) = 0; - R_board.at(2, 0) = 0; R_board.at(2, 1) = 0; R_board.at(2, 2) = 1; - } else { - // Rodrigues公式：R = I + sin(θ)/θ * K + (1-cos(θ))/θ² * K² - double rx = boardRoll / theta; - double ry = boardPitch / theta; - double rz = boardYaw / theta; - - double c = std::cos(theta); - double s = std::sin(theta); - double c1 = 1.0 - c; - - R_board.at(0, 0) = c + rx*rx*c1; - R_board.at(0, 1) = rx*ry*c1 - rz*s; - R_board.at(0, 2) = rx*rz*c1 + ry*s; - - R_board.at(1, 0) = ry*rx*c1 + rz*s; - R_board.at(1, 1) = c + ry*ry*c1; - R_board.at(1, 2) = ry*rz*c1 - rx*s; - - R_board.at(2, 0) = rz*rx*c1 - ry*s; - R_board.at(2, 1) = rz*ry*c1 + rx*s; - R_board.at(2, 2) = c + rz*rz*c1; - } + calib->EulerToRotationMatrix(boardAngles, eulerOrder, R_board); HECTranslationVector T_board(boardX, boardY, boardZ); data.boardInCamera = HECHomogeneousMatrix(R_board, T_board); @@ -807,15 +779,15 @@ void CalibDataWidget::setCameraInput(double x, double y, double z, m_inputCamY->setValue(y); m_inputCamZ->setValue(z); - // 外部传入的是弧度，如果当前输入单位是角度则转换显示 + // VrEyeView 回填的是欧拉角“度”，UI 根据当前输入单位决定如何显示 if (m_cbCamAngleUnit->currentIndex() == 1) { - m_inputCamRoll->setValue(rx * 180.0 / M_PI); - m_inputCamPitch->setValue(ry * 180.0 / M_PI); - m_inputCamYaw->setValue(rz * 180.0 / M_PI); - } else { m_inputCamRoll->setValue(rx); m_inputCamPitch->setValue(ry); m_inputCamYaw->setValue(rz); + } else { + m_inputCamRoll->setValue(rx * M_PI / 180.0); + m_inputCamPitch->setValue(ry * M_PI / 180.0); + m_inputCamYaw->setValue(rz * M_PI / 180.0); } } @@ -825,11 +797,26 @@ void CalibDataWidget::saveCalibData(const QString& filePath) const ini.setIniCodec("UTF-8"); int mode = m_cbCalibType->currentIndex(); + const int eulerOrderIndex = m_cbEulerOrder->currentIndex(); + QString axisOrder = "RzRyRx"; + switch (eulerOrderIndex) { + case 1: axisOrder = "RxRyRz"; break; + case 2: axisOrder = "RxRzRy"; break; + case 3: axisOrder = "RyRxRz"; break; + case 4: axisOrder = "RyRzRx"; break; + case 5: axisOrder = "RzRxRy"; break; + case 0: + default: + axisOrder = "RzRyRx"; + break; + } ini.beginGroup("CommInfo"); ini.setValue("eCalibType", mode); ini.setValue("sCalibTime", QDateTime::currentDateTime().toString("yyyy-MM-dd-HH-mm-ss")); ini.setValue("eCamAngleUnit", m_cbCamAngleUnit->currentIndex()); // 0=弧度, 1=角度 + ini.setValue("eEulerOrder", eulerOrderIndex); + ini.setValue("eRobotPosAxisOrder", axisOrder); if (mode == 0) { int count = m_tableHandEye->rowCount(); @@ -909,22 +896,27 @@ void CalibDataWidget::loadCalibData(const QString& filePath) int camAngleUnit = ini.value("eCamAngleUnit", 0).toInt(); m_cbCamAngleUnit->setCurrentIndex(camAngleUnit); - // 读取欧拉角顺序 - QString axisOrder = ini.value("eRobotPosAxisOrder", "RxRyRz").toString(); - - // 映射到UI的欧拉角顺序选择 - if (axisOrder == "RxRyRz") { - m_cbEulerOrder->setCurrentIndex(1); // XYZ - } else if (axisOrder == "RxRzRy") { - m_cbEulerOrder->setCurrentIndex(2); // XZY - } else if (axisOrder == "RyRxRz") { - m_cbEulerOrder->setCurrentIndex(3); // YXZ - } else if (axisOrder == "RyRzRx") { - m_cbEulerOrder->setCurrentIndex(4); // YZX - } else if (axisOrder == "RzRxRy") { - m_cbEulerOrder->setCurrentIndex(5); // ZXY + // 读取欧拉角顺序：优先使用当前工具保存的 eEulerOrder，兼容旧文件的 eRobotPosAxisOrder + int savedEulerOrder = ini.value("eEulerOrder", -1).toInt(); + if (savedEulerOrder >= 0 && savedEulerOrder <= 5) { + m_cbEulerOrder->setCurrentIndex(savedEulerOrder); } else { - m_cbEulerOrder->setCurrentIndex(0); // ZYX (默认) + QString axisOrder = ini.value("eRobotPosAxisOrder", "RzRyRx").toString(); + + // 映射到UI的欧拉角顺序选择 + if (axisOrder == "RxRyRz") { + m_cbEulerOrder->setCurrentIndex(1); // XYZ + } else if (axisOrder == "RxRzRy") { + m_cbEulerOrder->setCurrentIndex(2); // XZY + } else if (axisOrder == "RyRxRz") { + m_cbEulerOrder->setCurrentIndex(3); // YXZ + } else if (axisOrder == "RyRzRx") { + m_cbEulerOrder->setCurrentIndex(4); // YZX + } else if (axisOrder == "RzRxRy") { + m_cbEulerOrder->setCurrentIndex(5); // ZXY + } else { + m_cbEulerOrder->setCurrentIndex(0); // ZYX / RzRyRx + } } if (calibType == 0) { diff --git a/Tools/CalibView/Src/CalibViewMainWindow.cpp b/Tools/CalibView/Src/CalibViewMainWindow.cpp index ecd4744..b59904b 100644 --- a/Tools/CalibView/Src/CalibViewMainWindow.cpp +++ b/Tools/CalibView/Src/CalibViewMainWindow.cpp @@ -34,7 +34,19 @@ CalibViewMainWindow::CalibViewMainWindow(QWidget* parent) , m_sbRoll(nullptr) , m_sbPitch(nullptr) , m_sbYaw(nullptr) + , m_sbDirXX(nullptr) + , m_sbDirXY(nullptr) + , m_sbDirXZ(nullptr) + , m_sbDirYX(nullptr) + , m_sbDirYY(nullptr) + , m_sbDirYZ(nullptr) + , m_sbDirZX(nullptr) + , m_sbDirZY(nullptr) + , m_sbDirZZ(nullptr) + , m_cbAngleUnit(nullptr) , m_cbEulerOrder(nullptr) + , m_btnEulerToMatrix(nullptr) + , m_btnMatrixToEuler(nullptr) , m_btnTransform(nullptr) , m_logEdit(nullptr) , m_hasResult(false) @@ -108,16 +120,15 @@ QWidget* CalibViewMainWindow::createRightPanel() m_sbYaw->setDecimals(6); transformLayout->addWidget(m_sbYaw, 1, 5); - // 第3行：角度单位选择 + // 第3行：角度单位与欧拉角顺序 transformLayout->addWidget(new QLabel("角度单位:", this), 2, 0); m_cbAngleUnit = new QComboBox(this); m_cbAngleUnit->addItem("度(°)", 0); m_cbAngleUnit->addItem("弧度(rad)", 1); m_cbAngleUnit->setCurrentIndex(0); // 默认度 - transformLayout->addWidget(m_cbAngleUnit, 2, 1); + transformLayout->addWidget(m_cbAngleUnit, 2, 1, 1, 2); - // 第4行：欧拉角顺序 - transformLayout->addWidget(new QLabel("欧拉角顺序:", this), 3, 0); + transformLayout->addWidget(new QLabel("欧拉角顺序:", this), 2, 3); m_cbEulerOrder = new QComboBox(this); m_cbEulerOrder->addItem("XYZ", static_cast(HECEulerOrder::XYZ)); m_cbEulerOrder->addItem("XZY", static_cast(HECEulerOrder::XZY)); @@ -126,12 +137,74 @@ QWidget* CalibViewMainWindow::createRightPanel() m_cbEulerOrder->addItem("ZXY", static_cast(HECEulerOrder::ZXY)); m_cbEulerOrder->addItem("ZYX (常用)", static_cast(HECEulerOrder::ZYX)); m_cbEulerOrder->setCurrentIndex(5); // 默认 ZYX - transformLayout->addWidget(m_cbEulerOrder, 3, 1, 1, 2); + transformLayout->addWidget(m_cbEulerOrder, 2, 4, 1, 2); - // 第5行：变换按钮 + // 第4行：X轴方向向量（x, y, z） + transformLayout->addWidget(new QLabel("X轴(x,y,z):", this), 3, 0); + m_sbDirXX = new QDoubleSpinBox(this); + m_sbDirXX->setRange(-10000, 10000); + m_sbDirXX->setDecimals(6); + transformLayout->addWidget(m_sbDirXX, 3, 1); + + m_sbDirXY = new QDoubleSpinBox(this); + m_sbDirXY->setRange(-10000, 10000); + m_sbDirXY->setDecimals(6); + transformLayout->addWidget(m_sbDirXY, 3, 3); + + m_sbDirXZ = new QDoubleSpinBox(this); + m_sbDirXZ->setRange(-10000, 10000); + m_sbDirXZ->setDecimals(6); + transformLayout->addWidget(m_sbDirXZ, 3, 5); + + // 第5行：Y轴方向向量 + transformLayout->addWidget(new QLabel("Y轴(x,y,z):", this), 4, 0); + m_sbDirYX = new QDoubleSpinBox(this); + m_sbDirYX->setRange(-10000, 10000); + m_sbDirYX->setDecimals(6); + transformLayout->addWidget(m_sbDirYX, 4, 1); + + m_sbDirYY = new QDoubleSpinBox(this); + m_sbDirYY->setRange(-10000, 10000); + m_sbDirYY->setDecimals(6); + transformLayout->addWidget(m_sbDirYY, 4, 3); + + m_sbDirYZ = new QDoubleSpinBox(this); + m_sbDirYZ->setRange(-10000, 10000); + m_sbDirYZ->setDecimals(6); + transformLayout->addWidget(m_sbDirYZ, 4, 5); + + // 第6行：Z轴方向向量 + transformLayout->addWidget(new QLabel("Z轴(x,y,z):", this), 5, 0); + m_sbDirZX = new QDoubleSpinBox(this); + m_sbDirZX->setRange(-10000, 10000); + m_sbDirZX->setDecimals(6); + transformLayout->addWidget(m_sbDirZX, 5, 1); + + m_sbDirZY = new QDoubleSpinBox(this); + m_sbDirZY->setRange(-10000, 10000); + m_sbDirZY->setDecimals(6); + transformLayout->addWidget(m_sbDirZY, 5, 3); + + m_sbDirZZ = new QDoubleSpinBox(this); + m_sbDirZZ->setRange(-10000, 10000); + m_sbDirZZ->setDecimals(6); + transformLayout->addWidget(m_sbDirZZ, 5, 5); + + // 第7行：欧拉角与方向向量互转 + m_btnEulerToMatrix = new QPushButton("欧拉角 -> 向量", this); + connect(m_btnEulerToMatrix, &QPushButton::clicked, this, &CalibViewMainWindow::onEulerToDirectionMatrix); + transformLayout->addWidget(m_btnEulerToMatrix, 6, 0, 1, 3); + + m_btnMatrixToEuler = new QPushButton("向量 -> 欧拉角", this); + connect(m_btnMatrixToEuler, &QPushButton::clicked, this, &CalibViewMainWindow::onDirectionMatrixToEuler); + transformLayout->addWidget(m_btnMatrixToEuler, 6, 3, 1, 3); + + // 第8行：变换按钮 m_btnTransform = new QPushButton("变换", this); connect(m_btnTransform, &QPushButton::clicked, this, &CalibViewMainWindow::onTransformTest); - transformLayout->addWidget(m_btnTransform, 4, 0, 1, 6); + transformLayout->addWidget(m_btnTransform, 7, 0, 1, 6); + + updateDirectionMatrixDisplay(HECRotationMatrix()); rightLayout->addWidget(transformGroup); @@ -185,12 +258,12 @@ void CalibViewMainWindow::setupUI() QWidget* rightPanel = createRightPanel(); splitter->addWidget(rightPanel); - // 设置分割比例（左侧标定部分更宽，右侧测试栏更窄） + // 设置分割比例（右侧测试栏保持紧凑） splitter->setStretchFactor(0, 5); splitter->setStretchFactor(1, 1); // 设置初始大小 - splitter->setSizes(QList() << 1300 << 150); + splitter->setSizes(QList() << 1180 << 220); mainLayout->addWidget(splitter); setCentralWidget(centralWidget); @@ -281,7 +354,7 @@ void CalibViewMainWindow::createMenuBar() "- Eye-In-Hand 标定\n" "- TCP 标定\n" "- 坐标变换\n" - "- 欧拉角转换\n\n" + "- 方向向量与欧拉角互转\n\n" "基于 Eigen 库实现的 SVD 分解算法"); }); } @@ -296,6 +369,125 @@ void CalibViewMainWindow::appendLog(const QString& message) m_logEdit->append(message); } +void CalibViewMainWindow::updateDirectionMatrixDisplay(const HECRotationMatrix& R) +{ + if (!m_sbDirXX || !m_sbDirXY || !m_sbDirXZ || + !m_sbDirYX || !m_sbDirYY || !m_sbDirYZ || + !m_sbDirZX || !m_sbDirZY || !m_sbDirZZ) { + return; + } + + m_sbDirXX->setValue(R.at(0, 0)); + m_sbDirXY->setValue(R.at(1, 0)); + m_sbDirXZ->setValue(R.at(2, 0)); + + m_sbDirYX->setValue(R.at(0, 1)); + m_sbDirYY->setValue(R.at(1, 1)); + m_sbDirYZ->setValue(R.at(2, 1)); + + m_sbDirZX->setValue(R.at(0, 2)); + m_sbDirZY->setValue(R.at(1, 2)); + m_sbDirZZ->setValue(R.at(2, 2)); +} + +bool CalibViewMainWindow::tryGetDirectionMatrixInput(HECRotationMatrix& R) +{ + if (!m_sbDirXX || !m_sbDirXY || !m_sbDirXZ || + !m_sbDirYX || !m_sbDirYY || !m_sbDirYZ || + !m_sbDirZX || !m_sbDirZY || !m_sbDirZZ) { + return false; + } + + auto dot = [](const HECPoint3D& a, const HECPoint3D& b) { + return a.x * b.x + a.y * b.y + a.z * b.z; + }; + 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 + ); + }; + + HECPoint3D xAxis(m_sbDirXX->value(), m_sbDirXY->value(), m_sbDirXZ->value()); + HECPoint3D yAxis(m_sbDirYX->value(), m_sbDirYY->value(), m_sbDirYZ->value()); + HECPoint3D zAxis(m_sbDirZX->value(), m_sbDirZY->value(), m_sbDirZZ->value()); + + if (xAxis.norm() < 1e-10) { + QMessageBox::warning(this, "警告", "X轴方向向量长度不能为 0"); + return false; + } + if (yAxis.norm() < 1e-10) { + QMessageBox::warning(this, "警告", "Y轴方向向量长度不能为 0"); + return false; + } + if (zAxis.norm() < 1e-10) { + QMessageBox::warning(this, "警告", "Z轴方向向量长度不能为 0"); + return false; + } + + xAxis = xAxis.normalized(); + + HECPoint3D yAxisOrtho = yAxis - xAxis * dot(xAxis, yAxis); + if (yAxisOrtho.norm() < 1e-10) { + QMessageBox::warning(this, "警告", "X轴与Y轴方向向量不能共线"); + return false; + } + yAxisOrtho = yAxisOrtho.normalized(); + + HECPoint3D zAxisOrtho = cross(xAxis, yAxisOrtho); + if (zAxisOrtho.norm() < 1e-10) { + QMessageBox::warning(this, "警告", "无法根据 X/Y 轴方向向量构造正交坐标系"); + return false; + } + zAxisOrtho = zAxisOrtho.normalized(); + + HECPoint3D zAxisNorm = zAxis.normalized(); + const double scoreKeep = + dot(yAxisOrtho, yAxis.normalized()) + dot(zAxisOrtho, zAxisNorm); + const double scoreFlip = + dot(yAxisOrtho * (-1.0), yAxis.normalized()) + dot(zAxisOrtho * (-1.0), zAxisNorm); + if (scoreFlip > scoreKeep) { + yAxisOrtho = yAxisOrtho * (-1.0); + zAxisOrtho = zAxisOrtho * (-1.0); + } + + zAxisOrtho = zAxisOrtho - xAxis * dot(xAxis, zAxisOrtho); + zAxisOrtho = zAxisOrtho - yAxisOrtho * dot(yAxisOrtho, zAxisOrtho); + if (zAxisOrtho.norm() < 1e-10) { + QMessageBox::warning(this, "警告", "输入的方向向量无法稳定构造正交坐标系"); + return false; + } + zAxisOrtho = zAxisOrtho.normalized(); + yAxisOrtho = cross(zAxisOrtho, xAxis).normalized(); + + R.at(0, 0) = xAxis.x; + R.at(1, 0) = xAxis.y; + R.at(2, 0) = xAxis.z; + R.at(0, 1) = yAxisOrtho.x; + R.at(1, 1) = yAxisOrtho.y; + R.at(2, 1) = yAxisOrtho.z; + R.at(0, 2) = zAxisOrtho.x; + R.at(1, 2) = zAxisOrtho.y; + R.at(2, 2) = zAxisOrtho.z; + + return true; +} + +QString CalibViewMainWindow::eulerOrderToString(HECEulerOrder order) const +{ + switch (order) { + case HECEulerOrder::XYZ: return "XYZ"; + case HECEulerOrder::XZY: return "XZY"; + case HECEulerOrder::YXZ: return "YXZ"; + case HECEulerOrder::YZX: return "YZX"; + case HECEulerOrder::ZXY: return "ZXY"; + case HECEulerOrder::ZYX: return "ZYX"; + } + + return "ZYX"; +} + void CalibViewMainWindow::onEyeToHandCalib() { if (!m_calib) { @@ -488,6 +680,116 @@ void CalibViewMainWindow::onTCPCalib() } } +void CalibViewMainWindow::onEulerToDirectionMatrix() +{ + if (!m_calib) { + QMessageBox::critical(this, "错误", "标定实例未初始化"); + return; + } + + const bool isRadian = (m_cbAngleUnit->currentData().toInt() == 1); + const HECEulerAngles srcAngles = isRadian + ? HECEulerAngles(m_sbRoll->value(), m_sbPitch->value(), m_sbYaw->value()) + : HECEulerAngles::fromDegrees(m_sbRoll->value(), m_sbPitch->value(), m_sbYaw->value()); + const HECEulerOrder order = static_cast(m_cbEulerOrder->currentData().toInt()); + + HECRotationMatrix rotation; + m_calib->EulerToRotationMatrix(srcAngles, order, rotation); + updateDirectionMatrixDisplay(rotation); + + double rollDeg = 0.0; + double pitchDeg = 0.0; + double yawDeg = 0.0; + srcAngles.toDegrees(rollDeg, pitchDeg, yawDeg); + + appendLog(QString("欧拉角 -> 方向向量，顺序: %1").arg(eulerOrderToString(order))); + appendLog(QString(" Euler(rad): Roll=%1, Pitch=%2, Yaw=%3") + .arg(srcAngles.roll, 0, 'f', 6) + .arg(srcAngles.pitch, 0, 'f', 6) + .arg(srcAngles.yaw, 0, 'f', 6)); + appendLog(QString(" Euler(deg): Roll=%1°, Pitch=%2°, Yaw=%3°") + .arg(rollDeg, 0, 'f', 3) + .arg(pitchDeg, 0, 'f', 3) + .arg(yawDeg, 0, 'f', 3)); + appendLog(QString(" X轴方向: [%1, %2, %3]") + .arg(rotation.at(0, 0), 0, 'f', 6) + .arg(rotation.at(1, 0), 0, 'f', 6) + .arg(rotation.at(2, 0), 0, 'f', 6)); + appendLog(QString(" Y轴方向: [%1, %2, %3]") + .arg(rotation.at(0, 1), 0, 'f', 6) + .arg(rotation.at(1, 1), 0, 'f', 6) + .arg(rotation.at(2, 1), 0, 'f', 6)); + appendLog(QString(" Z轴方向: [%1, %2, %3]") + .arg(rotation.at(0, 2), 0, 'f', 6) + .arg(rotation.at(1, 2), 0, 'f', 6) + .arg(rotation.at(2, 2), 0, 'f', 6)); + + updateStatusBar("欧拉角转方向向量完成"); +} + +void CalibViewMainWindow::onDirectionMatrixToEuler() +{ + if (!m_calib) { + QMessageBox::critical(this, "错误", "标定实例未初始化"); + return; + } + + HECRotationMatrix rotation; + if (!tryGetDirectionMatrixInput(rotation)) { + return; + } + updateDirectionMatrixDisplay(rotation); + + const bool isRadian = (m_cbAngleUnit->currentData().toInt() == 1); + const HECEulerOrder order = static_cast(m_cbEulerOrder->currentData().toInt()); + + HECEulerAngles angles; + m_calib->RotationMatrixToEuler(rotation, order, angles); + + if (isRadian) { + m_sbRoll->setValue(angles.roll); + m_sbPitch->setValue(angles.pitch); + m_sbYaw->setValue(angles.yaw); + } else { + double rollDeg = 0.0; + double pitchDeg = 0.0; + double yawDeg = 0.0; + angles.toDegrees(rollDeg, pitchDeg, yawDeg); + m_sbRoll->setValue(rollDeg); + m_sbPitch->setValue(pitchDeg); + m_sbYaw->setValue(yawDeg); + } + + double rollDeg = 0.0; + double pitchDeg = 0.0; + double yawDeg = 0.0; + angles.toDegrees(rollDeg, pitchDeg, yawDeg); + + appendLog(QString("方向向量 -> 欧拉角，顺序: %1").arg(eulerOrderToString(order))); + appendLog(QString(" X轴方向: [%1, %2, %3]") + .arg(rotation.at(0, 0), 0, 'f', 6) + .arg(rotation.at(1, 0), 0, 'f', 6) + .arg(rotation.at(2, 0), 0, 'f', 6)); + appendLog(QString(" Y轴方向: [%1, %2, %3]") + .arg(rotation.at(0, 1), 0, 'f', 6) + .arg(rotation.at(1, 1), 0, 'f', 6) + .arg(rotation.at(2, 1), 0, 'f', 6)); + appendLog(QString(" Z轴方向: [%1, %2, %3]") + .arg(rotation.at(0, 2), 0, 'f', 6) + .arg(rotation.at(1, 2), 0, 'f', 6) + .arg(rotation.at(2, 2), 0, 'f', 6)); + appendLog(QString(" Euler(rad): Roll=%1, Pitch=%2, Yaw=%3") + .arg(angles.roll, 0, 'f', 6) + .arg(angles.pitch, 0, 'f', 6) + .arg(angles.yaw, 0, 'f', 6)); + appendLog(QString(" Euler(deg): Roll=%1°, Pitch=%2°, Yaw=%3°") + .arg(rollDeg, 0, 'f', 3) + .arg(pitchDeg, 0, 'f', 3) + .arg(yawDeg, 0, 'f', 3)); + + updateStatusBar("方向向量转欧拉角完成"); +} + void CalibViewMainWindow::onTransformTest() { if (!m_calib) { @@ -532,6 +834,7 @@ void CalibViewMainWindow::onTransformTest() // 欧拉角 -> 旋转矩阵 HECRotationMatrix R_src; m_calib->EulerToRotationMatrix(srcAngles, order, R_src); + updateDirectionMatrixDisplay(R_src); // 变换位置 HECPoint3D dstPoint; @@ -545,7 +848,6 @@ void CalibViewMainWindow::onTransformTest() m_calib->RotationMatrixToEuler(R_dst, order, dstAngles); // 输出日志 - QString unitStr = isRadian ? "rad" : "°"; appendLog(QString("坐标变换结果:")); appendLog(QString(" 源位置: (%1, %2, %3)") .arg(srcPoint.x, 0, 'f', 3) @@ -596,6 +898,7 @@ void CalibViewMainWindow::onClearAll() m_sbRoll->setValue(0); m_sbPitch->setValue(0); m_sbYaw->setValue(0); + updateDirectionMatrixDisplay(HECRotationMatrix()); m_hasResult = false; updateStatusBar("已清除所有数据"); }