461 lines
14 KiB
C++
461 lines
14 KiB
C++
#include "HandEyeCalibWidget.h"
|
|
#include "ui_HandEyeCalibWidget.h"
|
|
|
|
#include <QFileDialog>
|
|
#include <QFont>
|
|
#include <QList>
|
|
#include <QSettings>
|
|
#include <QVariant>
|
|
#include <cstring>
|
|
|
|
HandEyeCalibWidget::HandEyeCalibWidget(QWidget *parent)
|
|
: QWidget(parent)
|
|
, ui(new Ui::HandEyeCalibWidget)
|
|
, m_comboCamera(nullptr)
|
|
, m_labelStatus(nullptr)
|
|
, m_btnLoad(nullptr)
|
|
, m_btnSave(nullptr)
|
|
, m_matrixEditable(false)
|
|
{
|
|
memset(m_matrixEdits, 0, sizeof(m_matrixEdits));
|
|
setupUI();
|
|
}
|
|
|
|
HandEyeCalibWidget::~HandEyeCalibWidget()
|
|
{
|
|
delete ui;
|
|
}
|
|
|
|
void HandEyeCalibWidget::setCameraList(const QVector<HandEyeCalibCameraInfo>& cameras)
|
|
{
|
|
m_comboCamera->blockSignals(true);
|
|
m_comboCamera->clear();
|
|
|
|
for (const HandEyeCalibCameraInfo& camera : cameras) {
|
|
m_comboCamera->addItem(camera.displayName, QVariant(camera.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;
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void HandEyeCalibWidget::setExtrinsicData(int cameraIndex, int eulerOrder,
|
|
double rotX, double rotY, double rotZ,
|
|
double approachOffset,
|
|
double offsetX, double offsetY, double offsetZ)
|
|
{
|
|
HandEyeCalibData& data = ensureCalibData(cameraIndex);
|
|
data.eulerOrder = eulerOrder;
|
|
data.rotX = rotX;
|
|
data.rotY = rotY;
|
|
data.rotZ = rotZ;
|
|
data.approachOffset = approachOffset;
|
|
data.offsetX = offsetX;
|
|
data.offsetY = offsetY;
|
|
data.offsetZ = offsetZ;
|
|
}
|
|
|
|
bool HandEyeCalibWidget::getCalibData(int cameraIndex, double outMatrix[16], bool& outIsCalibrated) const
|
|
{
|
|
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
|
|
{
|
|
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;
|
|
}
|
|
|
|
const HandEyeCalibData* data = findCalibData(cameraIndex);
|
|
outApproachOffset = data ? data->approachOffset : 0.0;
|
|
return true;
|
|
}
|
|
|
|
bool HandEyeCalibWidget::getExtrinsicData(int cameraIndex, int& outEulerOrder,
|
|
double& outRotX, double& outRotY, double& outRotZ,
|
|
double& outApproachOffset,
|
|
double& outOffsetX, double& outOffsetY, double& outOffsetZ) const
|
|
{
|
|
if (!getExtrinsicData(cameraIndex, outEulerOrder, outRotX, outRotY, outRotZ, outApproachOffset)) {
|
|
return false;
|
|
}
|
|
|
|
const HandEyeCalibData* data = findCalibData(cameraIndex);
|
|
if (data) {
|
|
outOffsetX = data->offsetX;
|
|
outOffsetY = data->offsetY;
|
|
outOffsetZ = data->offsetZ;
|
|
} else {
|
|
outOffsetX = outOffsetY = outOffsetZ = 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)
|
|
{
|
|
Q_UNUSED(visible);
|
|
}
|
|
|
|
void HandEyeCalibWidget::setApproachOffsetVisible(bool visible)
|
|
{
|
|
Q_UNUSED(visible);
|
|
}
|
|
|
|
void HandEyeCalibWidget::setTargetOffsetVisible(bool visible)
|
|
{
|
|
Q_UNUSED(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);
|
|
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);
|
|
}
|
|
|
|
}
|
|
|
|
void HandEyeCalibWidget::onLoadCalibMatrixClicked()
|
|
{
|
|
int camIdx = currentCameraIndex();
|
|
if (camIdx < 0) {
|
|
return;
|
|
}
|
|
|
|
QString fileName = QFileDialog::getOpenFileName(this,
|
|
QStringLiteral("选择手眼标定矩阵文件"),
|
|
m_defaultFilePath,
|
|
QStringLiteral("INI Files (*.ini);;All Files (*)"));
|
|
|
|
if (fileName.isEmpty()) {
|
|
return;
|
|
}
|
|
|
|
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;
|
|
matrix[i] = settings.value(QString("dCalibMatrix_%1").arg(i), defaultVal).toDouble();
|
|
}
|
|
settings.endGroup();
|
|
|
|
setCalibData(camIdx, matrix, true);
|
|
emit calibMatrixLoaded(camIdx, matrix);
|
|
}
|
|
|
|
void HandEyeCalibWidget::onSaveCalibMatrixClicked()
|
|
{
|
|
int camIdx = currentCameraIndex();
|
|
if (camIdx < 0) {
|
|
return;
|
|
}
|
|
|
|
if (m_matrixEditable) {
|
|
double uiMatrix[16];
|
|
if (readMatrixFromUI(uiMatrix)) {
|
|
setCalibData(camIdx, uiMatrix, true);
|
|
}
|
|
}
|
|
|
|
const HandEyeCalibData* data = findCalibData(camIdx);
|
|
if (data && data->isCalibrated) {
|
|
emit saveCalibRequested(camIdx, data->matrix);
|
|
}
|
|
}
|
|
|
|
void HandEyeCalibWidget::setupUI()
|
|
{
|
|
ui->setupUi(this);
|
|
|
|
setStyleSheet(
|
|
"QLabel { color: rgb(221, 225, 233); }"
|
|
"QLineEdit { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); "
|
|
"border: 1px solid rgb(70, 72, 78); padding: 4px; }"
|
|
"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); }"
|
|
"QPushButton { color: rgb(221, 225, 233); border: none; border-radius: 4px; padding: 8px 16px; }"
|
|
"QPushButton#btnLoad { background-color: rgb(60, 120, 180); }"
|
|
"QPushButton#btnLoad:hover { background-color: rgb(80, 140, 200); }"
|
|
"QPushButton#btnLoad:pressed { background-color: rgb(40, 100, 160); }"
|
|
"QPushButton#btnSave { background-color: rgb(60, 150, 80); }"
|
|
"QPushButton#btnSave:hover { background-color: rgb(80, 170, 100); }"
|
|
"QPushButton#btnSave:pressed { background-color: rgb(40, 130, 60); }"
|
|
"QPushButton:disabled { background-color: rgb(80, 80, 80); color: rgb(120, 120, 120); }"
|
|
"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 controlFont;
|
|
controlFont.setPointSize(16);
|
|
const QList<QWidget*> widgets = findChildren<QWidget*>();
|
|
for (QWidget* widget : widgets) {
|
|
widget->setFont(controlFont);
|
|
}
|
|
|
|
QFont matrixFont;
|
|
matrixFont.setPointSize(16);
|
|
|
|
m_comboCamera = ui->comboCamera;
|
|
m_labelStatus = ui->labelStatus;
|
|
m_btnLoad = ui->btnLoad;
|
|
m_btnSave = ui->btnSave;
|
|
|
|
m_matrixEdits[0][0] = ui->editMatrix00;
|
|
m_matrixEdits[0][1] = ui->editMatrix01;
|
|
m_matrixEdits[0][2] = ui->editMatrix02;
|
|
m_matrixEdits[0][3] = ui->editMatrix03;
|
|
m_matrixEdits[1][0] = ui->editMatrix10;
|
|
m_matrixEdits[1][1] = ui->editMatrix11;
|
|
m_matrixEdits[1][2] = ui->editMatrix12;
|
|
m_matrixEdits[1][3] = ui->editMatrix13;
|
|
m_matrixEdits[2][0] = ui->editMatrix20;
|
|
m_matrixEdits[2][1] = ui->editMatrix21;
|
|
m_matrixEdits[2][2] = ui->editMatrix22;
|
|
m_matrixEdits[2][3] = ui->editMatrix23;
|
|
m_matrixEdits[3][0] = ui->editMatrix30;
|
|
m_matrixEdits[3][1] = ui->editMatrix31;
|
|
m_matrixEdits[3][2] = ui->editMatrix32;
|
|
m_matrixEdits[3][3] = ui->editMatrix33;
|
|
|
|
for (int r = 0; r < 4; ++r) {
|
|
for (int c = 0; c < 4; ++c) {
|
|
m_matrixEdits[r][c]->setFont(matrixFont);
|
|
m_matrixEdits[r][c]->setReadOnly(true);
|
|
}
|
|
}
|
|
|
|
QFont btnFont;
|
|
btnFont.setPointSize(16);
|
|
btnFont.setBold(true);
|
|
m_btnLoad->setFont(btnFont);
|
|
m_btnSave->setFont(btnFont);
|
|
m_btnLoad->setEnabled(false);
|
|
m_btnSave->setEnabled(false);
|
|
|
|
updateCalibStatus(false);
|
|
displayIdentityMatrix();
|
|
|
|
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::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]->clear();
|
|
}
|
|
}
|
|
}
|
|
|
|
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(QStringLiteral("标定状态: 已标定"));
|
|
m_labelStatus->setStyleSheet("color: rgb(100, 200, 100); padding: 4px 0;");
|
|
} else {
|
|
m_labelStatus->setText(QStringLiteral("标定状态: 未标定"));
|
|
m_labelStatus->setStyleSheet("color: rgb(180, 180, 180); padding: 4px 0;");
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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();
|
|
}
|