677 lines
23 KiB
C++
677 lines
23 KiB
C++
#include "HandEyeCalibWidget.h"
|
||
|
||
#include <QVBoxLayout>
|
||
#include <QHBoxLayout>
|
||
#include <QGridLayout>
|
||
#include <QFormLayout>
|
||
#include <QGroupBox>
|
||
#include <QDoubleValidator>
|
||
#include <QFileDialog>
|
||
#include <QSettings>
|
||
#include <cstring>
|
||
|
||
HandEyeCalibWidget::HandEyeCalibWidget(QWidget *parent)
|
||
: QWidget(parent)
|
||
, m_comboCamera(nullptr)
|
||
, m_labelStatus(nullptr)
|
||
, m_btnLoad(nullptr)
|
||
, m_btnSave(nullptr)
|
||
, m_groupExtrinsic(nullptr)
|
||
, m_comboEulerOrder(nullptr)
|
||
, m_editRotX(nullptr)
|
||
, m_editRotY(nullptr)
|
||
, m_editRotZ(nullptr)
|
||
, m_labelApproachOffset(nullptr)
|
||
, m_editApproachOffset(nullptr)
|
||
, m_matrixEditable(false)
|
||
{
|
||
memset(m_matrixEdits, 0, sizeof(m_matrixEdits));
|
||
setupUI();
|
||
}
|
||
|
||
HandEyeCalibWidget::~HandEyeCalibWidget()
|
||
{
|
||
}
|
||
|
||
// ========== 公开接口 ==========
|
||
|
||
void HandEyeCalibWidget::setCameraList(const QVector<HandEyeCalibCameraInfo>& cameras)
|
||
{
|
||
m_comboCamera->blockSignals(true);
|
||
m_comboCamera->clear();
|
||
|
||
for (int i = 0; i < cameras.size(); ++i) {
|
||
m_comboCamera->addItem(cameras[i].displayName, QVariant(cameras[i].cameraIndex));
|
||
}
|
||
|
||
m_comboCamera->blockSignals(false);
|
||
|
||
if (cameras.isEmpty()) {
|
||
onCameraSelectionChanged(-1);
|
||
} else {
|
||
// 默认选中第一个相机
|
||
m_comboCamera->setCurrentIndex(0);
|
||
onCameraSelectionChanged(0);
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::setCalibData(int cameraIndex, const double matrix[16], bool isCalibrated)
|
||
{
|
||
HandEyeCalibData& data = ensureCalibData(cameraIndex);
|
||
memcpy(data.matrix, matrix, sizeof(double) * 16);
|
||
data.isCalibrated = isCalibrated;
|
||
|
||
// 如果当前正在显示这个相机，刷新 UI
|
||
if (currentCameraIndex() == cameraIndex) {
|
||
if (isCalibrated) {
|
||
displayMatrix(matrix);
|
||
updateCalibStatus(true);
|
||
} else {
|
||
displayIdentityMatrix();
|
||
updateCalibStatus(false);
|
||
}
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::setExtrinsicData(int cameraIndex, int eulerOrder,
|
||
double rotX, double rotY, double rotZ)
|
||
{
|
||
HandEyeCalibData& data = ensureCalibData(cameraIndex);
|
||
data.eulerOrder = eulerOrder;
|
||
data.rotX = rotX;
|
||
data.rotY = rotY;
|
||
data.rotZ = rotZ;
|
||
|
||
if (currentCameraIndex() == cameraIndex) {
|
||
displayExtrinsic(data);
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::setExtrinsicData(int cameraIndex, int eulerOrder,
|
||
double rotX, double rotY, double rotZ,
|
||
double approachOffset)
|
||
{
|
||
HandEyeCalibData& data = ensureCalibData(cameraIndex);
|
||
data.eulerOrder = eulerOrder;
|
||
data.rotX = rotX;
|
||
data.rotY = rotY;
|
||
data.rotZ = rotZ;
|
||
data.approachOffset = approachOffset;
|
||
|
||
if (currentCameraIndex() == cameraIndex) {
|
||
displayExtrinsic(data);
|
||
}
|
||
}
|
||
|
||
bool HandEyeCalibWidget::getCalibData(int cameraIndex, double outMatrix[16], bool& outIsCalibrated) const
|
||
{
|
||
// 如果当前显示的就是这个相机，且矩阵可编辑，优先从 UI 读取
|
||
if (m_matrixEditable && currentCameraIndex() == cameraIndex) {
|
||
double uiMatrix[16];
|
||
if (readMatrixFromUI(uiMatrix)) {
|
||
memcpy(outMatrix, uiMatrix, sizeof(double) * 16);
|
||
outIsCalibrated = true;
|
||
return true;
|
||
}
|
||
}
|
||
|
||
const HandEyeCalibData* data = findCalibData(cameraIndex);
|
||
if (!data) {
|
||
return false;
|
||
}
|
||
|
||
memcpy(outMatrix, data->matrix, sizeof(double) * 16);
|
||
outIsCalibrated = data->isCalibrated;
|
||
return true;
|
||
}
|
||
|
||
bool HandEyeCalibWidget::getExtrinsicData(int cameraIndex, int& outEulerOrder,
|
||
double& outRotX, double& outRotY, double& outRotZ) const
|
||
{
|
||
// 当前相机若正在显示，优先读取 UI
|
||
if (currentCameraIndex() == cameraIndex && m_comboEulerOrder && m_editRotX && m_editRotY && m_editRotZ) {
|
||
outEulerOrder = m_comboEulerOrder->currentData().toInt();
|
||
outRotX = m_editRotX->text().trimmed().toDouble();
|
||
outRotY = m_editRotY->text().trimmed().toDouble();
|
||
outRotZ = m_editRotZ->text().trimmed().toDouble();
|
||
return true;
|
||
}
|
||
|
||
const HandEyeCalibData* data = findCalibData(cameraIndex);
|
||
if (!data) {
|
||
return false;
|
||
}
|
||
outEulerOrder = data->eulerOrder;
|
||
outRotX = data->rotX;
|
||
outRotY = data->rotY;
|
||
outRotZ = data->rotZ;
|
||
return true;
|
||
}
|
||
|
||
bool HandEyeCalibWidget::getExtrinsicData(int cameraIndex, int& outEulerOrder,
|
||
double& outRotX, double& outRotY, double& outRotZ,
|
||
double& outApproachOffset) const
|
||
{
|
||
if (!getExtrinsicData(cameraIndex, outEulerOrder, outRotX, outRotY, outRotZ)) {
|
||
return false;
|
||
}
|
||
|
||
// 接近点偏移：当前相机优先读 UI，否则读缓存
|
||
if (currentCameraIndex() == cameraIndex && m_editApproachOffset) {
|
||
outApproachOffset = m_editApproachOffset->text().trimmed().toDouble();
|
||
return true;
|
||
}
|
||
const HandEyeCalibData* data = findCalibData(cameraIndex);
|
||
outApproachOffset = data ? data->approachOffset : 0.0;
|
||
return true;
|
||
}
|
||
|
||
int HandEyeCalibWidget::currentCameraIndex() const
|
||
{
|
||
if (!m_comboCamera || m_comboCamera->count() == 0 || m_comboCamera->currentIndex() < 0) {
|
||
return -1;
|
||
}
|
||
return m_comboCamera->currentData().toInt();
|
||
}
|
||
|
||
void HandEyeCalibWidget::setDefaultFilePath(const QString& path)
|
||
{
|
||
m_defaultFilePath = path;
|
||
}
|
||
|
||
void HandEyeCalibWidget::setMatrixEditable(bool editable)
|
||
{
|
||
m_matrixEditable = editable;
|
||
for (int r = 0; r < 4; ++r) {
|
||
for (int c = 0; c < 4; ++c) {
|
||
if (m_matrixEdits[r][c]) {
|
||
m_matrixEdits[r][c]->setReadOnly(!editable);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::setExtrinsicControlsVisible(bool visible)
|
||
{
|
||
if (m_groupExtrinsic) {
|
||
m_groupExtrinsic->setVisible(visible);
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::setApproachOffsetVisible(bool visible)
|
||
{
|
||
if (m_labelApproachOffset) {
|
||
m_labelApproachOffset->setVisible(visible);
|
||
}
|
||
if (m_editApproachOffset) {
|
||
m_editApproachOffset->setVisible(visible);
|
||
}
|
||
}
|
||
|
||
// ========== 私有槽 ==========
|
||
|
||
void HandEyeCalibWidget::onCameraSelectionChanged(int index)
|
||
{
|
||
if (index < 0 || !m_comboCamera || m_comboCamera->count() == 0) {
|
||
m_btnLoad->setEnabled(false);
|
||
m_btnSave->setEnabled(false);
|
||
displayIdentityMatrix();
|
||
updateCalibStatus(false);
|
||
displayDefaultExtrinsic();
|
||
return;
|
||
}
|
||
|
||
m_btnLoad->setEnabled(true);
|
||
m_btnSave->setEnabled(true);
|
||
|
||
int camIdx = m_comboCamera->itemData(index).toInt();
|
||
const HandEyeCalibData* data = findCalibData(camIdx);
|
||
if (data && data->isCalibrated) {
|
||
displayMatrix(data->matrix);
|
||
updateCalibStatus(true);
|
||
} else {
|
||
displayIdentityMatrix();
|
||
updateCalibStatus(false);
|
||
}
|
||
if (data) {
|
||
displayExtrinsic(*data);
|
||
} else {
|
||
displayDefaultExtrinsic();
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::onLoadCalibMatrixClicked()
|
||
{
|
||
int camIdx = currentCameraIndex();
|
||
if (camIdx < 0) {
|
||
return;
|
||
}
|
||
|
||
QString startDir = m_defaultFilePath.isEmpty() ? QString() : m_defaultFilePath;
|
||
QString fileName = QFileDialog::getOpenFileName(this,
|
||
QString::fromUtf8("选择手眼标定矩阵文件"),
|
||
startDir,
|
||
QString::fromUtf8("INI Files (*.ini);;All Files (*)"));
|
||
|
||
if (fileName.isEmpty()) {
|
||
return;
|
||
}
|
||
|
||
// 用 QSettings 读取 [CalibMatrixInfo_0] section
|
||
QSettings settings(fileName, QSettings::IniFormat);
|
||
settings.setIniCodec("UTF-8");
|
||
settings.beginGroup("CalibMatrixInfo_0");
|
||
|
||
double matrix[16];
|
||
for (int i = 0; i < 16; ++i) {
|
||
int row = i / 4;
|
||
int col = i % 4;
|
||
double defaultVal = (row == col) ? 1.0 : 0.0;
|
||
QString key = QString("dCalibMatrix_%1").arg(i);
|
||
matrix[i] = settings.value(key, defaultVal).toDouble();
|
||
}
|
||
settings.endGroup();
|
||
|
||
// 更新缓存
|
||
setCalibData(camIdx, matrix, true);
|
||
|
||
// 发射信号通知外部
|
||
emit calibMatrixLoaded(camIdx, matrix);
|
||
}
|
||
|
||
void HandEyeCalibWidget::onSaveCalibMatrixClicked()
|
||
{
|
||
int camIdx = currentCameraIndex();
|
||
if (camIdx < 0) {
|
||
return;
|
||
}
|
||
|
||
// 如果可编辑，先从 UI 读取最新值更新到缓存
|
||
if (m_matrixEditable) {
|
||
double uiMatrix[16];
|
||
if (readMatrixFromUI(uiMatrix)) {
|
||
setCalibData(camIdx, uiMatrix, true);
|
||
}
|
||
}
|
||
|
||
// 无论矩阵是否可编辑，都同步外参到缓存
|
||
commitExtrinsicToCache(camIdx);
|
||
|
||
const HandEyeCalibData* data = findCalibData(camIdx);
|
||
if (data && data->isCalibrated) {
|
||
emit saveCalibRequested(camIdx, data->matrix);
|
||
}
|
||
}
|
||
|
||
// ========== 私有方法 ==========
|
||
|
||
void HandEyeCalibWidget::setupUI()
|
||
{
|
||
// 主样式（继承父容器暗色主题）
|
||
QString comboStyle =
|
||
"QComboBox { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); "
|
||
"border: 1px solid rgb(70, 72, 78); padding: 6px; }"
|
||
"QComboBox QAbstractItemView { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); "
|
||
"selection-background-color: rgb(70, 100, 150); }";
|
||
|
||
QString editStyle =
|
||
"QLineEdit { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); "
|
||
"border: 1px solid rgb(70, 72, 78); padding: 4px; }";
|
||
|
||
QString labelStyle = "color: rgb(221, 225, 233);";
|
||
|
||
QString btnLoadStyle =
|
||
"QPushButton { background-color: rgb(60, 120, 180); color: rgb(221, 225, 233); "
|
||
"border: none; border-radius: 4px; padding: 8px 16px; }"
|
||
"QPushButton:hover { background-color: rgb(80, 140, 200); }"
|
||
"QPushButton:pressed { background-color: rgb(40, 100, 160); }"
|
||
"QPushButton:disabled { background-color: rgb(80, 80, 80); color: rgb(120, 120, 120); }";
|
||
|
||
QString btnSaveStyle =
|
||
"QPushButton { background-color: rgb(60, 150, 80); color: rgb(221, 225, 233); "
|
||
"border: none; border-radius: 4px; padding: 8px 16px; }"
|
||
"QPushButton:hover { background-color: rgb(80, 170, 100); }"
|
||
"QPushButton:pressed { background-color: rgb(40, 130, 60); }"
|
||
"QPushButton:disabled { background-color: rgb(80, 80, 80); color: rgb(120, 120, 120); }";
|
||
|
||
QVBoxLayout* mainLayout = new QVBoxLayout(this);
|
||
mainLayout->setContentsMargins(20, 10, 20, 10);
|
||
|
||
// 相机选择行
|
||
QHBoxLayout* cameraRow = new QHBoxLayout();
|
||
QLabel* labelCamera = new QLabel(QString::fromUtf8("选择相机:"), this);
|
||
labelCamera->setStyleSheet(labelStyle);
|
||
QFont labelFont;
|
||
labelFont.setPointSize(16);
|
||
labelCamera->setFont(labelFont);
|
||
labelCamera->setMinimumWidth(120);
|
||
|
||
m_comboCamera = new QComboBox(this);
|
||
QFont comboFont;
|
||
comboFont.setPointSize(14);
|
||
m_comboCamera->setFont(comboFont);
|
||
m_comboCamera->setStyleSheet(comboStyle);
|
||
|
||
cameraRow->addWidget(labelCamera);
|
||
cameraRow->addWidget(m_comboCamera);
|
||
mainLayout->addLayout(cameraRow);
|
||
|
||
// 标定状态
|
||
m_labelStatus = new QLabel(QString::fromUtf8("标定状态: 未标定"), this);
|
||
QFont statusFont;
|
||
statusFont.setPointSize(14);
|
||
m_labelStatus->setFont(statusFont);
|
||
m_labelStatus->setStyleSheet("color: rgb(180, 180, 180); padding: 4px 0;");
|
||
mainLayout->addWidget(m_labelStatus);
|
||
|
||
// 矩阵标题
|
||
QLabel* labelMatrixTitle = new QLabel(QString::fromUtf8("4x4 变换矩阵:"), this);
|
||
QFont titleFont;
|
||
titleFont.setPointSize(14);
|
||
labelMatrixTitle->setFont(titleFont);
|
||
labelMatrixTitle->setStyleSheet(labelStyle);
|
||
mainLayout->addWidget(labelMatrixTitle);
|
||
|
||
// 4x4 矩阵 Grid
|
||
QGridLayout* gridLayout = new QGridLayout();
|
||
QFont editFont;
|
||
editFont.setPointSize(12);
|
||
for (int r = 0; r < 4; ++r) {
|
||
for (int c = 0; c < 4; ++c) {
|
||
m_matrixEdits[r][c] = new QLineEdit(this);
|
||
m_matrixEdits[r][c]->setFont(editFont);
|
||
m_matrixEdits[r][c]->setReadOnly(true); // 默认只读
|
||
m_matrixEdits[r][c]->setStyleSheet(editStyle);
|
||
gridLayout->addWidget(m_matrixEdits[r][c], r, c);
|
||
}
|
||
}
|
||
mainLayout->addLayout(gridLayout);
|
||
|
||
// 可选外参区（欧拉角顺序 + 补偿旋转）
|
||
setupExtrinsicGroup();
|
||
|
||
// 按钮行
|
||
QHBoxLayout* btnRow = new QHBoxLayout();
|
||
m_btnLoad = new QPushButton(QString::fromUtf8("加载标定矩阵"), this);
|
||
QFont btnFont;
|
||
btnFont.setPointSize(14);
|
||
btnFont.setBold(true);
|
||
m_btnLoad->setFont(btnFont);
|
||
m_btnLoad->setStyleSheet(btnLoadStyle);
|
||
m_btnLoad->setEnabled(false);
|
||
|
||
m_btnSave = new QPushButton(QString::fromUtf8("保存标定"), this);
|
||
m_btnSave->setFont(btnFont);
|
||
m_btnSave->setStyleSheet(btnSaveStyle);
|
||
m_btnSave->setEnabled(false);
|
||
|
||
btnRow->addWidget(m_btnLoad);
|
||
btnRow->addWidget(m_btnSave);
|
||
mainLayout->addLayout(btnRow);
|
||
|
||
if (m_groupExtrinsic) {
|
||
mainLayout->addWidget(m_groupExtrinsic);
|
||
m_groupExtrinsic->setVisible(false); // 默认隐藏，调用方按需开启
|
||
}
|
||
|
||
// 底部弹性空间
|
||
mainLayout->addStretch();
|
||
|
||
// 连接信号
|
||
connect(m_comboCamera, static_cast<void(QComboBox::*)(int)>(&QComboBox::currentIndexChanged),
|
||
this, &HandEyeCalibWidget::onCameraSelectionChanged);
|
||
connect(m_btnLoad, &QPushButton::clicked,
|
||
this, &HandEyeCalibWidget::onLoadCalibMatrixClicked);
|
||
connect(m_btnSave, &QPushButton::clicked,
|
||
this, &HandEyeCalibWidget::onSaveCalibMatrixClicked);
|
||
}
|
||
|
||
void HandEyeCalibWidget::setupExtrinsicGroup()
|
||
{
|
||
const QString labelStyle = "color: rgb(221, 225, 233);";
|
||
const QString editStyle =
|
||
"QLineEdit { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); "
|
||
"border: 1px solid rgb(70, 72, 78); padding: 4px; }";
|
||
const QString comboStyle =
|
||
"QComboBox { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); "
|
||
"border: 1px solid rgb(70, 72, 78); padding: 6px; }"
|
||
"QComboBox QAbstractItemView { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); "
|
||
"selection-background-color: rgb(70, 100, 150); }";
|
||
const QString groupStyle =
|
||
"QGroupBox { color: rgb(221, 225, 233); border: 1px solid rgb(100, 100, 100); "
|
||
"border-radius: 4px; margin-top: 12px; padding-top: 8px; }"
|
||
"QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px; }";
|
||
|
||
QFont font;
|
||
font.setPointSize(14);
|
||
|
||
m_groupExtrinsic = new QGroupBox(QString::fromUtf8("欧拉角顺序 / 工具坐标系补偿旋转"), this);
|
||
m_groupExtrinsic->setFont(font);
|
||
m_groupExtrinsic->setStyleSheet(groupStyle);
|
||
|
||
QFormLayout* form = new QFormLayout(m_groupExtrinsic);
|
||
form->setSpacing(8);
|
||
|
||
m_comboEulerOrder = new QComboBox(this);
|
||
m_comboEulerOrder->setFont(font);
|
||
m_comboEulerOrder->setStyleSheet(comboStyle);
|
||
initEulerOrderComboBox();
|
||
QLabel* labelEuler = new QLabel(QString::fromUtf8("欧拉角旋转顺序:"), this);
|
||
labelEuler->setFont(font);
|
||
labelEuler->setStyleSheet(labelStyle);
|
||
form->addRow(labelEuler, m_comboEulerOrder);
|
||
|
||
auto* validatorX = new QDoubleValidator(-360.0, 360.0, 6, this);
|
||
validatorX->setNotation(QDoubleValidator::StandardNotation);
|
||
auto* validatorY = new QDoubleValidator(-360.0, 360.0, 6, this);
|
||
validatorY->setNotation(QDoubleValidator::StandardNotation);
|
||
auto* validatorZ = new QDoubleValidator(-360.0, 360.0, 6, this);
|
||
validatorZ->setNotation(QDoubleValidator::StandardNotation);
|
||
|
||
m_editRotX = new QLineEdit(this);
|
||
m_editRotX->setFont(font);
|
||
m_editRotX->setStyleSheet(editStyle);
|
||
m_editRotX->setValidator(validatorX);
|
||
|
||
m_editRotY = new QLineEdit(this);
|
||
m_editRotY->setFont(font);
|
||
m_editRotY->setStyleSheet(editStyle);
|
||
m_editRotY->setValidator(validatorY);
|
||
|
||
m_editRotZ = new QLineEdit(this);
|
||
m_editRotZ->setFont(font);
|
||
m_editRotZ->setStyleSheet(editStyle);
|
||
m_editRotZ->setValidator(validatorZ);
|
||
|
||
auto makeAxisLabel = [&](const QString& text) {
|
||
QLabel* label = new QLabel(text, this);
|
||
label->setFont(font);
|
||
label->setStyleSheet(labelStyle);
|
||
return label;
|
||
};
|
||
|
||
QWidget* rotationRow = new QWidget(this);
|
||
QHBoxLayout* rotationLayout = new QHBoxLayout(rotationRow);
|
||
rotationLayout->setContentsMargins(0, 0, 0, 0);
|
||
rotationLayout->setSpacing(8);
|
||
rotationLayout->addWidget(makeAxisLabel(QString::fromUtf8("X:")));
|
||
rotationLayout->addWidget(m_editRotX, 1);
|
||
rotationLayout->addWidget(makeAxisLabel(QString::fromUtf8("Y:")));
|
||
rotationLayout->addWidget(m_editRotY, 1);
|
||
rotationLayout->addWidget(makeAxisLabel(QString::fromUtf8("Z:")));
|
||
rotationLayout->addWidget(m_editRotZ, 1);
|
||
|
||
QLabel* labelRotation = new QLabel(QString::fromUtf8("姿态调整 (°):"), this);
|
||
labelRotation->setFont(font);
|
||
labelRotation->setStyleSheet(labelStyle);
|
||
form->addRow(labelRotation, rotationRow);
|
||
|
||
auto* validatorOffset = new QDoubleValidator(-10000.0, 10000.0, 3, this);
|
||
validatorOffset->setNotation(QDoubleValidator::StandardNotation);
|
||
m_editApproachOffset = new QLineEdit(this);
|
||
m_editApproachOffset->setFont(font);
|
||
m_editApproachOffset->setStyleSheet(editStyle);
|
||
m_editApproachOffset->setValidator(validatorOffset);
|
||
|
||
m_labelApproachOffset = new QLabel(QString::fromUtf8("接近点偏移 (mm):"), this);
|
||
m_labelApproachOffset->setFont(font);
|
||
m_labelApproachOffset->setStyleSheet(labelStyle);
|
||
form->addRow(m_labelApproachOffset, m_editApproachOffset);
|
||
|
||
// 默认隐藏，仅需要 approach 点的 App 调 setApproachOffsetVisible(true) 打开
|
||
m_labelApproachOffset->setVisible(false);
|
||
m_editApproachOffset->setVisible(false);
|
||
|
||
displayDefaultExtrinsic();
|
||
}
|
||
|
||
void HandEyeCalibWidget::initEulerOrderComboBox()
|
||
{
|
||
if (!m_comboEulerOrder) return;
|
||
// 外旋 (绕固定轴)：CTEulerOrder sXYZ=10 ... sXZY=15
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RZ-RY-RX (外旋ZYX)"), 11);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RX-RY-RZ (外旋XYZ)"), 10);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RZ-RX-RY (外旋ZXY)"), 12);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RY-RX-RZ (外旋YXZ)"), 13);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RY-RZ-RX (外旋YZX)"), 14);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RX-RZ-RY (外旋XZY)"), 15);
|
||
// 内旋 (绕旋转后的新轴)：CTEulerOrder XYZ=0 ... XZY=5
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RZ-RY-RX (内旋ZYX)"), 1);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RX-RY-RZ (内旋XYZ)"), 0);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RZ-RX-RY (内旋ZXY)"), 2);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RY-RX-RZ (内旋YXZ)"), 3);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RY-RZ-RX (内旋YZX)"), 4);
|
||
m_comboEulerOrder->addItem(QString::fromUtf8("RX-RZ-RY (内旋XZY)"), 5);
|
||
m_comboEulerOrder->setCurrentIndex(0);
|
||
}
|
||
|
||
void HandEyeCalibWidget::displayMatrix(const double* matrix)
|
||
{
|
||
if (!matrix) return;
|
||
|
||
for (int r = 0; r < 4; ++r) {
|
||
for (int c = 0; c < 4; ++c) {
|
||
m_matrixEdits[r][c]->setText(QString::number(matrix[r * 4 + c], 'f', 6));
|
||
}
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::clearMatrix()
|
||
{
|
||
for (int r = 0; r < 4; ++r) {
|
||
for (int c = 0; c < 4; ++c) {
|
||
m_matrixEdits[r][c]->setText("");
|
||
}
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::displayIdentityMatrix()
|
||
{
|
||
static const double identity[16] = {
|
||
1.0, 0.0, 0.0, 0.0,
|
||
0.0, 1.0, 0.0, 0.0,
|
||
0.0, 0.0, 1.0, 0.0,
|
||
0.0, 0.0, 0.0, 1.0
|
||
};
|
||
displayMatrix(identity);
|
||
}
|
||
|
||
void HandEyeCalibWidget::updateCalibStatus(bool isCalibrated)
|
||
{
|
||
if (isCalibrated) {
|
||
m_labelStatus->setText(QString::fromUtf8("标定状态: 已标定"));
|
||
m_labelStatus->setStyleSheet("color: rgb(100, 200, 100); padding: 4px 0;");
|
||
} else {
|
||
m_labelStatus->setText(QString::fromUtf8("标定状态: 未标定"));
|
||
m_labelStatus->setStyleSheet("color: rgb(180, 180, 180); padding: 4px 0;");
|
||
}
|
||
}
|
||
|
||
void HandEyeCalibWidget::displayExtrinsic(const HandEyeCalibData& data)
|
||
{
|
||
if (m_comboEulerOrder) {
|
||
int idx = m_comboEulerOrder->findData(data.eulerOrder);
|
||
if (idx < 0) idx = 0;
|
||
m_comboEulerOrder->setCurrentIndex(idx);
|
||
}
|
||
if (m_editRotX) m_editRotX->setText(QString::number(data.rotX, 'f', 6));
|
||
if (m_editRotY) m_editRotY->setText(QString::number(data.rotY, 'f', 6));
|
||
if (m_editRotZ) m_editRotZ->setText(QString::number(data.rotZ, 'f', 6));
|
||
if (m_editApproachOffset) m_editApproachOffset->setText(QString::number(data.approachOffset, 'f', 3));
|
||
}
|
||
|
||
void HandEyeCalibWidget::displayDefaultExtrinsic()
|
||
{
|
||
if (m_comboEulerOrder) {
|
||
int idx = m_comboEulerOrder->findData(11);
|
||
if (idx < 0) idx = 0;
|
||
m_comboEulerOrder->setCurrentIndex(idx);
|
||
}
|
||
if (m_editRotX) m_editRotX->setText(QString::number(0.0, 'f', 6));
|
||
if (m_editRotY) m_editRotY->setText(QString::number(0.0, 'f', 6));
|
||
if (m_editRotZ) m_editRotZ->setText(QString::number(0.0, 'f', 6));
|
||
if (m_editApproachOffset) m_editApproachOffset->setText(QString::number(0.0, 'f', 3));
|
||
}
|
||
|
||
bool HandEyeCalibWidget::readMatrixFromUI(double outMatrix[16]) const
|
||
{
|
||
for (int r = 0; r < 4; ++r) {
|
||
for (int c = 0; c < 4; ++c) {
|
||
if (!m_matrixEdits[r][c]) return false;
|
||
QString text = m_matrixEdits[r][c]->text().trimmed();
|
||
if (text.isEmpty()) return false;
|
||
bool ok = false;
|
||
outMatrix[r * 4 + c] = text.toDouble(&ok);
|
||
if (!ok) return false;
|
||
}
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void HandEyeCalibWidget::commitExtrinsicToCache(int cameraIndex)
|
||
{
|
||
if (!m_comboEulerOrder || !m_editRotX || !m_editRotY || !m_editRotZ) {
|
||
return;
|
||
}
|
||
HandEyeCalibData& data = ensureCalibData(cameraIndex);
|
||
data.eulerOrder = m_comboEulerOrder->currentData().toInt();
|
||
data.rotX = m_editRotX->text().trimmed().toDouble();
|
||
data.rotY = m_editRotY->text().trimmed().toDouble();
|
||
data.rotZ = m_editRotZ->text().trimmed().toDouble();
|
||
if (m_editApproachOffset) {
|
||
data.approachOffset = m_editApproachOffset->text().trimmed().toDouble();
|
||
}
|
||
}
|
||
|
||
HandEyeCalibData* HandEyeCalibWidget::findCalibData(int cameraIndex)
|
||
{
|
||
for (int i = 0; i < m_calibDataCache.size(); ++i) {
|
||
if (m_calibDataCache[i].cameraIndex == cameraIndex) {
|
||
return &m_calibDataCache[i];
|
||
}
|
||
}
|
||
return nullptr;
|
||
}
|
||
|
||
const HandEyeCalibData* HandEyeCalibWidget::findCalibData(int cameraIndex) const
|
||
{
|
||
for (int i = 0; i < m_calibDataCache.size(); ++i) {
|
||
if (m_calibDataCache[i].cameraIndex == cameraIndex) {
|
||
return &m_calibDataCache[i];
|
||
}
|
||
}
|
||
return nullptr;
|
||
}
|
||
|
||
HandEyeCalibData& HandEyeCalibWidget::ensureCalibData(int cameraIndex)
|
||
{
|
||
HandEyeCalibData* existing = findCalibData(cameraIndex);
|
||
if (existing) {
|
||
return *existing;
|
||
}
|
||
HandEyeCalibData newData;
|
||
newData.cameraIndex = cameraIndex;
|
||
m_calibDataCache.append(newData);
|
||
return m_calibDataCache.last();
|
||
}
|