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

棒材的定位姿态问题，增加长边的轴配置

Browse Source
This commit is contained in:
yiyi 2026-03-29 20:37:36 +08:00
parent f0de4a15da
commit 839862ea26
15 changed files with 223 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Inc/DetectPresenter.h
View File

@ -35,6 +35,7 @@ public:
const double clibMatrix[16],
int eulerOrder,
int dirVectorInvert,
int longAxisDir,
DetectionResult& detectionResult);
};

165
App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp
View File

@ -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<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) {
@ -161,6 +65,7 @@ int DetectPresenter::DetectRod(
const double clibMatrix[16],
int eulerOrder,
int dirVectorInvert,
int longAxisDir,
DetectionResult& detectionResult)
{
if (laserLines.empty()) {
@ -350,61 +255,43 @@ int DetectPresenter::DetectRod(
for (size_t i = 0; i < rodInfo.size(); i++) {
const auto& rod = rodInfo[i];
// 进行坐标转换：从算法坐标系转换到机械臂坐标系
SVzNL3DPoint targetObj;
targetObj.x = rod.center.x;
targetObj.y = rod.center.y;
targetObj.z = rod.center.z;
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);
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);
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<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);
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){

5
App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp
View File

@ -215,14 +215,15 @@ int RodAndBarPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzRe
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);
const int longAxisDir = configResult.longAxisDir;
LOG_INFO("[Algo Thread] Using euler order: %d, dir vector invert: %d, long axis dir: %d\n", eulerOrder, dirVectorInvert, longAxisDir);
DetectionResult detectionResult;
int nRet = m_pDetectPresenter->DetectRod(m_currentCameraIndex, detectionDataCache,
algorithmParams, debugParam, m_dataLoader,
currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert,
detectionResult);
longAxisDir, detectionResult);
// 根据项目类型选择处理方式
if (GetStatusCallback<IYRodAndBarPositionStatus>()) {
QString err = QString("错误:%1").arg(nRet);

2
App/RodAndBarPosition/RodAndBarPositionApp/Version.h
View File

@ -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

3
App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp
View File

@ -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;
}

5
App/RodAndBarPosition/RodAndBarPositionConfig/Inc/IVrConfig.h
View File

@ -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) {
}
// 默认构造函数

6
App/RodAndBarPosition/RodAndBarPositionConfig/Src/VrConfig.cpp
View File

@ -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. 保存手眼标定矩阵列表（支持多相机）

2
App/RodAndBarPosition/RodAndBarPositionConfig/config/config.xml
View File

@ -42,7 +42,7 @@
<!-- 串口配置 -->
<SerialConfig portName="COM6" baudRate="115200" dataBits="8" stopBits="1" parity="0" flowControl="0" enabled="false" />
<!-- 网络参数 -->
<NetworkConfig eulerOrder="11" dirVectorInvert="0" byteOrder="0" />
<NetworkConfig eulerOrder="11" dirVectorInvert="0" byteOrder="0" longAxisDir="0" />
<!-- 手眼标定矩阵 -->
<HandEyeCalibMatrixs>
<HandEyeCalibMatrix cameraIndex="1" m0="1" m1="0" m2="0" m3="0" m4="0" m5="1" m6="0" m7="0" m8="0" m9="0" m10="1" m11="0" m12="0" m13="0" m14="0" m15="1" eulerOrder="11"/>

4
AppUtils/UICommon/Inc/NetworkConfigWidget.h
View File

@ -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;

24
AppUtils/UICommon/Src/NetworkConfigWidget.cpp
View File

@ -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(); }

1
Module/HandEyeCalib/HandEyeCalib.pro
View File

@ -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 {

10
Module/HandEyeCalib/Inc/HandEyeCalibTypes.h
View File

@ -72,6 +72,16 @@ enum class HECCalibrationType
EyeInHand // 眼在手上：相机安装在末端，求相机到末端的变换
};
/**
* @brief 目标物长边对应的坐标轴
* 决定构建坐标系时哪个方向向量作为长边（主轴）种子
*/
enum class HECLongAxisDir
{
AxisX = 0, // 长边对应 X 轴（inputDirA 作为 X 轴种子）
AxisY = 1 // 长边对应 Y 轴（inputDirA 作为 Y 轴种子）
};
/**
* @brief 欧拉角旋转顺序枚举（外旋 extrinsic）
* 外旋：所有旋转都绕固定的世界坐标系轴进行

26
Module/HandEyeCalib/Inc/IHandEyeCalib.h
View File

@ -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 眼坐标系下的点集

106
Module/HandEyeCalib/Src/HandEyeCalib.cpp
View File

@ -4,6 +4,7 @@
#include "HandEyeCalib.h"
#include "VrError.h"
#include "VrLog.h"
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <cmath>
@ -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<int>(longAxisMapping), eyeRollDeg, eyePitchDeg, eyeYawDeg);
std::vector<HECPoint3D> 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<HECPoint3D> 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<HECPoint3D>& eyePoints,
const std::vector<HECPoint3D>& robotPoints,

10
Module/HandEyeCalib/_Inc/HandEyeCalib.h
View File

@ -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<HECPoint3D>& eyePoints,
const std::vector<HECPoint3D>& robotPoints,