eye to robot
This commit is contained in:
yiyi
parent
b5ef4a0a5d
commit
f0de4a15da
@ -33,6 +33,8 @@ public:
|
||||
const VrDebugParam& debugParam,
|
||||
LaserDataLoader& dataLoader,
|
||||
const double clibMatrix[16],
|
||||
int eulerOrder,
|
||||
int dirVectorInvert,
|
||||
DetectionResult& detectionResult);
|
||||
|
||||
};
|
||||
|
||||
@ -1,13 +1,143 @@
|
||||
#include "DetectPresenter.h"
|
||||
#include "rodAndBarDetection_Export.h"
|
||||
#include "AlgoParamConverter.h"
|
||||
#include "IHandEyeCalib.h"
|
||||
#include <fstream>
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <QPainter>
|
||||
#include <QPen>
|
||||
#include <QColor>
|
||||
|
||||
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<HECPoint3D>& 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<HECPoint3D>& 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<HECPoint3D>& 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<IHandEyeCalib, decltype(&DestroyHandEyeCalibInstance)> 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<HECPoint3D> 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<HECPoint3D> 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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -207,12 +207,22 @@ int RodAndBarPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzRe
|
||||
VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
|
||||
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<int>(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<IYRodAndBarPositionStatus>()) {
|
||||
QString err = QString("错误:%1").arg(nRet);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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;
|
||||
|
||||
// 右侧面板 - 日志
|
||||
|
||||
@ -480,8 +480,8 @@ void CalibDataWidget::getEyeToHandPoseData(std::vector<HECEyeToHandData>& 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<HECEyeToHandData>& 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) {
|
||||
|
||||
@ -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<int>(HECEulerOrder::XYZ));
|
||||
m_cbEulerOrder->addItem("XZY", static_cast<int>(HECEulerOrder::XZY));
|
||||
@ -126,12 +137,74 @@ QWidget* CalibViewMainWindow::createRightPanel()
|
||||
m_cbEulerOrder->addItem("ZXY", static_cast<int>(HECEulerOrder::ZXY));
|
||||
m_cbEulerOrder->addItem("ZYX (常用)", static_cast<int>(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<int>() << 1300 << 150);
|
||||
splitter->setSizes(QList<int>() << 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<HECEulerOrder>(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<HECEulerOrder>(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("已清除所有数据");
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user