diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h index 11855de9..1951ff84 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h @@ -35,6 +35,7 @@ public: const double clibMatrix[16], int eulerOrder, int dirVectorInvert, + int longAxisDir, DetectionResult& detectionResult); }; diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp index 14ec7447..78bf6189 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp @@ -13,102 +13,6 @@ 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) { @@ -161,6 +65,7 @@ int DetectPresenter::DetectRod( const double clibMatrix[16], int eulerOrder, int dirVectorInvert, + int longAxisDir, DetectionResult& detectionResult) { if (laserLines.empty()) { @@ -349,62 +254,44 @@ int DetectPresenter::DetectRod( // 转换检测结果为UI显示格式(使用机械臂坐标系数据) for (size_t i = 0; i < rodInfo.size(); i++) { const auto& rod = rodInfo[i]; + + LOG_INFO("[Algo Thread] Rod %zu Eye Center: X=%.2f, Y=%.2f, Z=%.2f\n", i, rod.center.x, rod.center.y, rod.center.z); + LOG_INFO("[Algo Thread] Rod %zu Input X seed: [%.6f, %.6f, %.6f]\n", i, rod.axialDir.x, rod.axialDir.y, rod.axialDir.z); + LOG_INFO("[Algo Thread] Rod %zu Input Z seed: [%.6f, %.6f, %.6f]\n", i, rod.normalDir.x, rod.normalDir.y, rod.normalDir.z); - // 进行坐标转换:从算法坐标系转换到机械臂坐标系 - SVzNL3DPoint targetObj; - targetObj.x = rod.center.x; - targetObj.y = rod.center.y; - targetObj.z = rod.center.z; - - 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); + HECPoseResult poseResult; + bool validPose = handEyeCalib->TransformPose( + calibResult, + HECPoint3D(rod.center.x, rod.center.y, rod.center.z), + HECPoint3D(rod.axialDir.x, rod.axialDir.y, rod.axialDir.z), + HECPoint3D(rod.normalDir.x, rod.normalDir.y, rod.normalDir.z), + dirVectorInvert, + hecEulerOrder, + longAxisDir == 1 ? HECLongAxisDir::AxisY : HECLongAxisDir::AxisX, + poseResult); 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); + double rollDeg = 0.0, pitchDeg = 0.0, yawDeg = 0.0; + poseResult.angles.toDegrees(rollDeg, pitchDeg, yawDeg); // 创建位置数据(使用转换后的机械臂坐标) RodPosition pos; pos.roll = rollDeg; pos.pitch = pitchDeg; pos.yaw = yawDeg; - pos.x = robotPoint.x; // 机械臂坐标X - pos.y = robotPoint.y; // 机械臂坐标Y - pos.z = robotPoint.z; // 机械臂坐标Z + pos.x = poseResult.position.x; + pos.y = poseResult.position.y; + pos.z = poseResult.position.z; detectionResult.positions.push_back(pos); // 保存棒材信息 RodInfo info; - info.centerX = robotPoint.x; - info.centerY = robotPoint.y; - info.centerZ = robotPoint.z; + info.centerX = poseResult.position.x; + info.centerY = poseResult.position.y; + info.centerZ = poseResult.position.z; info.axialDirX = rod.axialDir.x; info.axialDirY = rod.axialDir.y; info.axialDirZ = rod.axialDir.z; @@ -419,12 +306,8 @@ int DetectPresenter::DetectRod( info.endPtZ = rod.endPt.z; 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); - } + // Print key values for coordinate transform debugging + LOG_INFO("[Algo Thread] Rod %zu Robot Pose: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.6f, Pitch=%.6f, Yaw=%.6f\n", i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw); } if(debugParam.enableDebug && debugParam.saveDebugImage){ diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp index db73f817..764e4890 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp @@ -215,14 +215,15 @@ int RodAndBarPositionPresenter::ProcessAlgoDetection(std::vectorDetectRod(m_currentCameraIndex, detectionDataCache, algorithmParams, debugParam, m_dataLoader, currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, - detectionResult); + longAxisDir, detectionResult); // 根据项目类型选择处理方式 if (GetStatusCallback()) { QString err = QString("错误:%1").arg(nRet); diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Version.h b/App/RodAndBarPosition/RodAndBarPositionApp/Version.h index 0963b134..67f98735 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Version.h +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Version.h @@ -5,7 +5,7 @@ // 应用名称 #define RODANDBARPOSITION_APP_NAME "棒材定位" -#define RODANDBARPOSITION_VERSION_STRING "1.0.1" +#define RODANDBARPOSITION_VERSION_STRING "1.0.2" #define RODANDBARPOSITION_BUILD_STRING "1" #define RODANDBARPOSITION_FULL_VERSION_STRING "V" RODANDBARPOSITION_VERSION_STRING "_" RODANDBARPOSITION_BUILD_STRING diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp b/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp index d0e94757..a2d9e5b2 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp +++ b/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp @@ -116,6 +116,7 @@ void DialogAlgoArg::saveParams() systemConfig.configResult.eulerOrder = netConfig.eulerOrder; systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert; systemConfig.configResult.byteOrder = netConfig.byteOrder; + systemConfig.configResult.longAxisDir = netConfig.longAxisDir; } if (hasNetworkConfig) { for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) { @@ -320,6 +321,7 @@ void DialogAlgoArg::loadNetworkConfig() } netConfig.dirVectorInvert = configResult.dirVectorInvert; netConfig.byteOrder = configResult.byteOrder; + netConfig.longAxisDir = configResult.longAxisDir; m_networkConfigWidget->setConfig(netConfig); } @@ -338,6 +340,7 @@ void DialogAlgoArg::saveNetworkConfig() systemConfig.configResult.eulerOrder = netConfig.eulerOrder; systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert; systemConfig.configResult.byteOrder = netConfig.byteOrder; + systemConfig.configResult.longAxisDir = netConfig.longAxisDir; for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) { calibMatrix.eulerOrder = netConfig.eulerOrder; } diff --git a/App/RodAndBarPosition/RodAndBarPositionConfig/Inc/IVrConfig.h b/App/RodAndBarPosition/RodAndBarPositionConfig/Inc/IVrConfig.h index 7dc628a3..cc53c74e 100644 --- a/App/RodAndBarPosition/RodAndBarPositionConfig/Inc/IVrConfig.h +++ b/App/RodAndBarPosition/RodAndBarPositionConfig/Inc/IVrConfig.h @@ -78,6 +78,7 @@ struct ConfigResult int eulerOrder = 11; // 欧拉角旋转顺序,默认11=外旋ZYX int dirVectorInvert = 0; // 方向向量反向,默认0=不反向 int byteOrder = 0; // 数据字节序,默认0=大端序 + int longAxisDir = 0; // 目标物长边对应轴:0=X轴, 1=Y轴 // 显式赋值构造函数,确保正确的深拷贝 ConfigResult& operator=(const ConfigResult& other) { @@ -90,6 +91,7 @@ struct ConfigResult eulerOrder = other.eulerOrder; dirVectorInvert = other.dirVectorInvert; byteOrder = other.byteOrder; + longAxisDir = other.longAxisDir; } return *this; } @@ -103,7 +105,8 @@ struct ConfigResult , handEyeCalibMatrixList(other.handEyeCalibMatrixList) , eulerOrder(other.eulerOrder) , dirVectorInvert(other.dirVectorInvert) - , byteOrder(other.byteOrder) { + , byteOrder(other.byteOrder) + , longAxisDir(other.longAxisDir) { } // 默认构造函数 diff --git a/App/RodAndBarPosition/RodAndBarPositionConfig/Src/VrConfig.cpp b/App/RodAndBarPosition/RodAndBarPositionConfig/Src/VrConfig.cpp index 7a034bee..611de2df 100644 --- a/App/RodAndBarPosition/RodAndBarPositionConfig/Src/VrConfig.cpp +++ b/App/RodAndBarPosition/RodAndBarPositionConfig/Src/VrConfig.cpp @@ -117,9 +117,10 @@ int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResul networkElement->QueryIntAttribute("eulerOrder", &configResult.eulerOrder); networkElement->QueryIntAttribute("dirVectorInvert", &configResult.dirVectorInvert); networkElement->QueryIntAttribute("byteOrder", &configResult.byteOrder); + networkElement->QueryIntAttribute("longAxisDir", &configResult.longAxisDir); - LOG_INFO("Network config: eulerOrder=%d, dirVectorInvert=%d, byteOrder=%d\n", - configResult.eulerOrder, configResult.dirVectorInvert, configResult.byteOrder); + LOG_INFO("Network config: eulerOrder=%d, dirVectorInvert=%d, byteOrder=%d, longAxisDir=%d\n", + configResult.eulerOrder, configResult.dirVectorInvert, configResult.byteOrder, configResult.longAxisDir); } // 6. 解析手眼标定矩阵列表(支持多相机) @@ -195,6 +196,7 @@ bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResu networkElement->SetAttribute("eulerOrder", configResult.eulerOrder); networkElement->SetAttribute("dirVectorInvert", configResult.dirVectorInvert); networkElement->SetAttribute("byteOrder", configResult.byteOrder); + networkElement->SetAttribute("longAxisDir", configResult.longAxisDir); root->InsertEndChild(networkElement); // 6. 保存手眼标定矩阵列表(支持多相机) diff --git a/App/RodAndBarPosition/RodAndBarPositionConfig/config/config.xml b/App/RodAndBarPosition/RodAndBarPositionConfig/config/config.xml index f3dcc72c..d4e7f2d1 100644 --- a/App/RodAndBarPosition/RodAndBarPositionConfig/config/config.xml +++ b/App/RodAndBarPosition/RodAndBarPositionConfig/config/config.xml @@ -42,7 +42,7 @@ - + diff --git a/AppUtils/UICommon/Inc/NetworkConfigWidget.h b/AppUtils/UICommon/Inc/NetworkConfigWidget.h index cb2b4bf2..886ef5e9 100644 --- a/AppUtils/UICommon/Inc/NetworkConfigWidget.h +++ b/AppUtils/UICommon/Inc/NetworkConfigWidget.h @@ -15,6 +15,7 @@ struct NetworkConfigData int eulerOrder = 11; // 欧拉角旋转顺序,默认11=外旋ZYX int dirVectorInvert = 0; // 方向向量反向,默认0=不反向 int byteOrder = 0; // 字节序,默认0=大端序 + int longAxisDir = 0; // 目标物长边对应轴:0=X轴, 1=Y轴 // PLC服务端配置(可选) QString plcServerIp = "192.168.0.88"; @@ -98,18 +99,21 @@ public: int eulerOrder() const; int dirVectorInvert() const; int byteOrder() const; + int longAxisDir() const; private: void setupUI(bool showPlcConfig, bool showTcpConfig); void initEulerOrderComboBox(); void initDirVectorInvertComboBox(); void initByteOrderComboBox(); + void initLongAxisDirComboBox(); private: // 通用配置控件 QComboBox* m_comboEulerOrder; QComboBox* m_comboDirVectorInvert; QComboBox* m_comboByteOrder; + QComboBox* m_comboLongAxisDir; // PLC服务端配置控件(可选) QGroupBox* m_groupPlc; diff --git a/AppUtils/UICommon/Src/NetworkConfigWidget.cpp b/AppUtils/UICommon/Src/NetworkConfigWidget.cpp index 613acd84..ef1a9312 100644 --- a/AppUtils/UICommon/Src/NetworkConfigWidget.cpp +++ b/AppUtils/UICommon/Src/NetworkConfigWidget.cpp @@ -9,6 +9,7 @@ NetworkConfigWidget::NetworkConfigWidget(bool showPlcConfig, bool showTcpConfig, , m_comboEulerOrder(nullptr) , m_comboDirVectorInvert(nullptr) , m_comboByteOrder(nullptr) + , m_comboLongAxisDir(nullptr) , m_groupPlc(nullptr) , m_editPlcIp(nullptr) , m_editPlcPort(nullptr) @@ -66,6 +67,16 @@ void NetworkConfigWidget::setupUI(bool showPlcConfig, bool showTcpConfig) labelDir->setStyleSheet(labelStyle); commonForm->addRow(labelDir, m_comboDirVectorInvert); + // 目标物长边对应轴 + m_comboLongAxisDir = new QComboBox(this); + m_comboLongAxisDir->setFont(font); + m_comboLongAxisDir->setStyleSheet(comboStyle); + initLongAxisDirComboBox(); + QLabel* labelLongAxis = new QLabel("长边对应轴:"); + labelLongAxis->setFont(font); + labelLongAxis->setStyleSheet(labelStyle); + commonForm->addRow(labelLongAxis, m_comboLongAxisDir); + // 数据字节序 m_comboByteOrder = new QComboBox(this); m_comboByteOrder->setFont(font); @@ -166,6 +177,13 @@ void NetworkConfigWidget::initByteOrderComboBox() m_comboByteOrder->setCurrentIndex(0); // 默认大端序 } +void NetworkConfigWidget::initLongAxisDirComboBox() +{ + m_comboLongAxisDir->addItem("X轴", 0); + m_comboLongAxisDir->addItem("Y轴", 1); + m_comboLongAxisDir->setCurrentIndex(0); // 默认X轴 +} + void NetworkConfigWidget::setConfig(const NetworkConfigData& config) { // 欧拉角旋转顺序 @@ -176,6 +194,10 @@ void NetworkConfigWidget::setConfig(const NetworkConfigData& config) idx = m_comboDirVectorInvert->findData(config.dirVectorInvert); if (idx >= 0) m_comboDirVectorInvert->setCurrentIndex(idx); + // 长边对应轴 + idx = m_comboLongAxisDir->findData(config.longAxisDir); + if (idx >= 0) m_comboLongAxisDir->setCurrentIndex(idx); + // 字节序 idx = m_comboByteOrder->findData(config.byteOrder); if (idx >= 0) m_comboByteOrder->setCurrentIndex(idx); @@ -197,6 +219,7 @@ NetworkConfigData NetworkConfigWidget::getConfig() const config.eulerOrder = m_comboEulerOrder->currentData().toInt(); config.dirVectorInvert = m_comboDirVectorInvert->currentData().toInt(); config.byteOrder = m_comboByteOrder->currentData().toInt(); + config.longAxisDir = m_comboLongAxisDir->currentData().toInt(); if (m_editPlcIp) config.plcServerIp = m_editPlcIp->text().trimmed(); if (m_editPlcPort) config.plcServerPort = m_editPlcPort->text().toInt(); @@ -211,3 +234,4 @@ NetworkConfigData NetworkConfigWidget::getConfig() const int NetworkConfigWidget::eulerOrder() const { return m_comboEulerOrder->currentData().toInt(); } int NetworkConfigWidget::dirVectorInvert() const { return m_comboDirVectorInvert->currentData().toInt(); } int NetworkConfigWidget::byteOrder() const { return m_comboByteOrder->currentData().toInt(); } +int NetworkConfigWidget::longAxisDir() const { return m_comboLongAxisDir->currentData().toInt(); } diff --git a/Module/HandEyeCalib/HandEyeCalib.pro b/Module/HandEyeCalib/HandEyeCalib.pro index 9b3e6e74..758fd3dd 100644 --- a/Module/HandEyeCalib/HandEyeCalib.pro +++ b/Module/HandEyeCalib/HandEyeCalib.pro @@ -32,6 +32,7 @@ INCLUDEPATH += $$PWD/../../SDK/eigen-3.3.9 # common inc INCLUDEPATH += $$PWD/../../Utils/VrCommon/Inc +INCLUDEPATH += $$PWD/../../Utils/VrUtils/Inc # Default rules for deployment. unix { diff --git a/Module/HandEyeCalib/Inc/HandEyeCalibTypes.h b/Module/HandEyeCalib/Inc/HandEyeCalibTypes.h index 9170de97..c8b49f3c 100644 --- a/Module/HandEyeCalib/Inc/HandEyeCalibTypes.h +++ b/Module/HandEyeCalib/Inc/HandEyeCalibTypes.h @@ -72,6 +72,16 @@ enum class HECCalibrationType EyeInHand // 眼在手上:相机安装在末端,求相机到末端的变换 }; +/** + * @brief 目标物长边对应的坐标轴 + * 决定构建坐标系时哪个方向向量作为长边(主轴)种子 + */ +enum class HECLongAxisDir +{ + AxisX = 0, // 长边对应 X 轴(inputDirA 作为 X 轴种子) + AxisY = 1 // 长边对应 Y 轴(inputDirA 作为 Y 轴种子) +}; + /** * @brief 欧拉角旋转顺序枚举(外旋 extrinsic) * 外旋:所有旋转都绕固定的世界坐标系轴进行 diff --git a/Module/HandEyeCalib/Inc/IHandEyeCalib.h b/Module/HandEyeCalib/Inc/IHandEyeCalib.h index 135fa9bb..f9f0b08a 100644 --- a/Module/HandEyeCalib/Inc/IHandEyeCalib.h +++ b/Module/HandEyeCalib/Inc/IHandEyeCalib.h @@ -136,6 +136,32 @@ public: bool invertYZ, HECPoseResult& poseResult) = 0; + /** + * @brief Convert eye-frame center + XZ seed directions to robot-frame pose. + * + * Builds a right-handed frame from xSeed (X axis) and zSeed (Z axis), + * applies the dirVectorInvert flip, transforms position and orientation through + * the hand-eye calibration, and returns the result in degrees. + * + * @param calibResult Hand-eye calibration result (R, T) + * @param eyeCenter Object center in eye (camera) frame + * @param xSeed X-axis seed direction in eye frame + * @param zSeed Z-axis seed direction in eye frame + * @param dirVectorInvert Axis-flip mode: 0=none, 1=flip XY, 2=flip XZ, 3=flip YZ + * @param eulerOrder Euler angle decomposition order + * @param poseResult Output robot-frame position (mm) and Euler angles (degrees) + * @return true if the seed vectors form a valid frame, false otherwise + */ + virtual bool TransformPose( + const HECCalibResult& calibResult, + const HECPoint3D& eyeCenter, + const HECPoint3D& longAxisDir, + const HECPoint3D& normalDir, + int dirVectorInvert, + HECEulerOrder eulerOrder, + HECLongAxisDir longAxisMapping, + HECPoseResult& poseResult) = 0; + /** * @brief 计算标定误差 * @param eyePoints 眼坐标系下的点集 diff --git a/Module/HandEyeCalib/Src/HandEyeCalib.cpp b/Module/HandEyeCalib/Src/HandEyeCalib.cpp index ab8f8dcb..6983dd94 100644 --- a/Module/HandEyeCalib/Src/HandEyeCalib.cpp +++ b/Module/HandEyeCalib/Src/HandEyeCalib.cpp @@ -4,6 +4,7 @@ #include "HandEyeCalib.h" #include "VrError.h" +#include "VrLog.h" #include #include #include @@ -321,6 +322,111 @@ void HandEyeCalib::TransformPose( RotationMatrixToEulerZYX(R_pose, poseResult.angles); } +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(longAxisMapping), eyeRollDeg, eyePitchDeg, eyeYawDeg); + + std::vector 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 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& eyePoints, const std::vector& robotPoints, diff --git a/Module/HandEyeCalib/_Inc/HandEyeCalib.h b/Module/HandEyeCalib/_Inc/HandEyeCalib.h index 34aab5b8..8560af95 100644 --- a/Module/HandEyeCalib/_Inc/HandEyeCalib.h +++ b/Module/HandEyeCalib/_Inc/HandEyeCalib.h @@ -67,6 +67,16 @@ public: bool invertYZ, HECPoseResult& poseResult) override; + bool TransformPose( + const HECCalibResult& calibResult, + const HECPoint3D& eyeCenter, + const HECPoint3D& longAxisDir, + const HECPoint3D& normalDir, + int dirVectorInvert, + HECEulerOrder eulerOrder, + HECLongAxisDir longAxisMapping, + HECPoseResult& poseResult) override; + double CalculateError( const std::vector& eyePoints, const std::vector& robotPoints,