first Commit
This commit is contained in:
@@ -0,0 +1,279 @@
|
||||
#pragma once
|
||||
// ════════════════════════════════════════════════════════════════════════════
|
||||
// HandEyeCalibDialog.h ─ 在线手眼标定工具
|
||||
//
|
||||
// 功能:
|
||||
// 1. 采集/加载图像 + 输入 TCP 位姿 → 组成观测对
|
||||
// 2. 支持 眼在手上(eye-in-hand) / 眼在手外(eye-to-hand)
|
||||
// 3. 3D手眼标定 AX=XB: Tsai-Lenz / Park / Horaud / Daniilidis
|
||||
// 4. 标定效果评价: 旋转残差、平移残差、AX-XB一致性
|
||||
// 5. 导出4×4齐次变换矩阵
|
||||
// ════════════════════════════════════════════════════════════════════════════
|
||||
#include <QDialog>
|
||||
#include <QList>
|
||||
#include <QImage>
|
||||
#include <QString>
|
||||
#include <QtGlobal>
|
||||
#include <memory>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
class CameraManager;
|
||||
class QLabel;
|
||||
class QTreeWidget;
|
||||
class QTreeWidgetItem;
|
||||
class QLineEdit;
|
||||
class QPushButton;
|
||||
class QProgressBar;
|
||||
class QTextEdit;
|
||||
class QComboBox;
|
||||
class QDoubleSpinBox;
|
||||
class QGroupBox;
|
||||
class QCheckBox;
|
||||
|
||||
// ── 单条观测数据 ─────────────────────────────────────────────────────────
|
||||
struct HandEyeObservation {
|
||||
int index = 0;
|
||||
QString imagePath;
|
||||
|
||||
// 当前 TOOL 的 TCP 位姿(世界或固定用户参考坐标系下)
|
||||
double robotX = 0, robotY = 0, robotZ = 0; // mm
|
||||
// ESTUN XYZABC: A/B/C 分别绕 X/Y/Z;R = Rz(C) * Ry(B) * Rx(A)。
|
||||
double robotA = 0, robotB = 0, robotC = 0; // deg
|
||||
|
||||
// 标定板检测结果
|
||||
bool boardDetected = false;
|
||||
// 相机→标定板位姿 (由 VisionGuide 检测得到)
|
||||
double camPoseData[7] = {}; // VisionGuide 7-element pose
|
||||
|
||||
// 公开数据集或函数库可直接提供4x4矩阵,避免欧拉角约定造成二次转换误差。
|
||||
cv::Mat T_reference_tcp; // ^reference T_TCP,平移单位 m
|
||||
cv::Mat T_target2cam; // ^camera T_target,平移单位 m
|
||||
bool robotMatrixValid = false;
|
||||
bool targetMatrixValid = false;
|
||||
double reprojectionErrorPx = -1.0;
|
||||
|
||||
// 在线工程采集的时间信息。时间戳由中控机与本机使用同一时钟时,
|
||||
// synchronizationSkewMs 才具有绝对意义;否则仍可用于追溯采集顺序。
|
||||
qint64 poseTimestampMs = 0;
|
||||
qint64 captureTimestampMs = 0;
|
||||
qint64 synchronizationSkewMs = -1;
|
||||
QString poseSource;
|
||||
|
||||
bool excluded = false;
|
||||
|
||||
// 评价
|
||||
double convergenceError = -1.0;
|
||||
|
||||
QString displayName() const {
|
||||
return QString("obs_%1").arg(index, 3, 10, QChar('0'));
|
||||
}
|
||||
QString statusStr() const {
|
||||
if (excluded) return QString::fromUtf8("已排除");
|
||||
if (!boardDetected) return QString::fromUtf8("未检测");
|
||||
return QString::fromUtf8("有效");
|
||||
}
|
||||
QString errorStr() const {
|
||||
if (convergenceError >= 0)
|
||||
return QString::number(convergenceError, 'f', 4) + " mm";
|
||||
if (reprojectionErrorPx >= 0)
|
||||
return QString::number(reprojectionErrorPx, 'f', 4) + " px";
|
||||
return "-";
|
||||
}
|
||||
};
|
||||
|
||||
// ── 手眼标定结果 ─────────────────────────────────────────────────────────
|
||||
struct HandEyeResult {
|
||||
bool valid = false;
|
||||
cv::Mat R_hand_eye; // 3×3 旋转矩阵
|
||||
cv::Mat t_hand_eye; // 3×1 平移向量
|
||||
cv::Mat T_4x4; // 4×4 齐次变换矩阵
|
||||
|
||||
double rotationError = -1.0; // 平均旋转残差 (deg)
|
||||
double translationError = -1.0; // 平均平移残差 (mm)
|
||||
double convergenceRMS = -1.0; // AX-XB 一致性 RMS
|
||||
double closureRotationRMS = -1.0; // 闭环固定坐标系姿态 RMS (deg)
|
||||
double closureTranslationRMS = -1.0; // 闭环固定坐标系位置 RMS (mm)
|
||||
double groundTruthRotationError = -1.0; // deg
|
||||
double groundTruthTranslationError = -1.0; // mm
|
||||
|
||||
bool qualityPassed = false;
|
||||
QString qualityMessage;
|
||||
int observationCount = 0;
|
||||
QString transformConvention;
|
||||
|
||||
QString algorithmName;
|
||||
QString handEyeType; // "eye_in_hand" or "eye_to_hand"
|
||||
QString dataSource;
|
||||
};
|
||||
|
||||
// ════════════════════════════════════════════════════════════════════════════
|
||||
class HandEyeCalibDialog : public QDialog
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
explicit HandEyeCalibDialog(CameraManager* camMgr, QWidget* parent = nullptr);
|
||||
~HandEyeCalibDialog();
|
||||
|
||||
bool hasResult() const { return m_result.valid; }
|
||||
QString resultFilePath() const { return m_resultFile; }
|
||||
HandEyeResult result() const { return m_result; }
|
||||
|
||||
static QString defaultSaveDir();
|
||||
|
||||
public slots:
|
||||
// 工程接口:中控机函数库收到测量时刻 TCP 位姿后调用本槽,并在同一次调用中拍照。
|
||||
// 位姿约定为 ^reference T_TCP,平移 mm,旋转为 ESTUN ABC(deg):
|
||||
// R = Rz(C) * Ry(B) * Rx(A)(内禀 XYZ / 外禀 ZYX)。
|
||||
// 从非GUI线程调用时会自动排队到本对象线程执行。
|
||||
void captureSynchronizedObservation(double xMm, double yMm, double zMm,
|
||||
double aDeg, double bDeg, double cDeg,
|
||||
qint64 poseTimestampMs = 0);
|
||||
|
||||
signals:
|
||||
void calibrationDone(const QString& matrixFile);
|
||||
void observationCaptureFinished(bool success, int observationIndex,
|
||||
const QString& message);
|
||||
|
||||
private slots:
|
||||
// 配置
|
||||
void onBrowseCaltab();
|
||||
void onBrowseSaveDir();
|
||||
void onLoadCamParam();
|
||||
|
||||
// 采集
|
||||
void onCaptureAndInput();
|
||||
void onLoadImageBatch();
|
||||
void onLoadRobotPosesFile();
|
||||
void onLoadPublicDataset();
|
||||
void onEditRobotPose();
|
||||
|
||||
// 列表操作
|
||||
void onObsClicked(QTreeWidgetItem* item, int col);
|
||||
void onExcludeSelected();
|
||||
void onIncludeSelected();
|
||||
void onDeleteSelected();
|
||||
|
||||
// 检测
|
||||
void onValidateAll();
|
||||
|
||||
// 标定
|
||||
void onCalibrate();
|
||||
void onCompareAlgorithms();
|
||||
void onComputeErrors();
|
||||
void onSave();
|
||||
|
||||
private:
|
||||
void buildUi();
|
||||
void setupConnections();
|
||||
void refreshList();
|
||||
void displayObs(int idx);
|
||||
void displayImageOnLabel(QLabel* lbl, const QImage& img);
|
||||
void log(const QString& msg);
|
||||
void setProgress(int pct, const QString& msg = {});
|
||||
|
||||
bool validateSingleObs(int idx);
|
||||
bool runCalibration(const QList<int>& usedIndices);
|
||||
bool solveCalibration(const QList<int>& usedIndices,
|
||||
int algorithm,
|
||||
const QString& algorithmName,
|
||||
HandEyeResult& result,
|
||||
QString* errorMessage = nullptr) const;
|
||||
bool solveVisionCalibration(const QList<int>& usedIndices,
|
||||
HandEyeResult& result,
|
||||
QString* errorMessage = nullptr);
|
||||
bool importRwheDataset(const QString& datasetDir);
|
||||
bool observationTransforms(const HandEyeObservation& obs,
|
||||
cv::Mat& T_reference_tcp,
|
||||
cv::Mat& T_target2cam) const;
|
||||
bool captureObservation(const cv::Mat& T_reference_tcp,
|
||||
qint64 poseTimestampMs,
|
||||
const QString& poseSource,
|
||||
QString* errorMessage = nullptr);
|
||||
bool validateObservationSet(const QList<int>& usedIndices,
|
||||
QString& report) const;
|
||||
void invalidateResult(const QString& reason = {});
|
||||
void clearValidationDatasetContext();
|
||||
void evaluateAXXB(const QList<int>& usedIndices, HandEyeResult& result) const;
|
||||
void evaluateClosureConsistency(const QList<int>& usedIndices,
|
||||
HandEyeResult& result) const;
|
||||
void displayCalibrationResult(const HandEyeResult& result);
|
||||
bool exportComparisonCsv(const QList<HandEyeResult>& results, QString& pathOut) const;
|
||||
bool exportObservationCsv(QString& pathOut) const;
|
||||
void computePerPoseErrors(const QList<int>& usedIndices);
|
||||
|
||||
// VisionGuide 标定板检测 → 相机位姿
|
||||
bool detectBoardPose(const QImage& img, double poseOut[7],
|
||||
double* reprojectionErrorPx = nullptr);
|
||||
void updateBoardParameterUi();
|
||||
void invalidateBoardDetections(const QString& reason);
|
||||
|
||||
// 辅助: VisionGuide pose ↔ OpenCV R,t 转换
|
||||
static bool VisionGuidePoseToRt(const double pose[7],
|
||||
cv::Mat& R, cv::Mat& t);
|
||||
static void estunPoseToRt(double x, double y, double z,
|
||||
double a, double b, double c,
|
||||
cv::Mat& R, cv::Mat& t);
|
||||
static void matrixToEstunPose(const cv::Mat& T,
|
||||
double& x, double& y, double& z,
|
||||
double& a, double& b, double& c);
|
||||
static bool isRigidTransform(const cv::Mat& T, QString* reason = nullptr);
|
||||
static QString transformConventionForType(const QString& handEyeType);
|
||||
// 评价指标计算
|
||||
void computeAXXBConsistency(const QList<int>& usedIndices);
|
||||
|
||||
CameraManager* m_camMgr;
|
||||
|
||||
QString m_saveDir;
|
||||
QList<HandEyeObservation> m_observations;
|
||||
HandEyeResult m_result;
|
||||
QString m_resultFile;
|
||||
|
||||
// Top widgets
|
||||
QLineEdit* m_editCaltab = nullptr;
|
||||
QLineEdit* m_editSaveDir = nullptr;
|
||||
QLineEdit* m_editCamParam = nullptr;
|
||||
QComboBox* m_comboHandEyeType = nullptr;
|
||||
QComboBox* m_comboBoardType = nullptr;
|
||||
QDoubleSpinBox* m_spinFindSigma = nullptr;
|
||||
QDoubleSpinBox* m_spinFindAlpha = nullptr;
|
||||
QDoubleSpinBox* m_spinGapTolerance = nullptr;
|
||||
QDoubleSpinBox* m_spinMaxDiamMarks = nullptr;
|
||||
QCheckBox* m_checkSkipFindCaltab = nullptr;
|
||||
|
||||
// Robot pose input
|
||||
QDoubleSpinBox* m_spinRX = nullptr;
|
||||
QDoubleSpinBox* m_spinRY = nullptr;
|
||||
QDoubleSpinBox* m_spinRZ = nullptr;
|
||||
QDoubleSpinBox* m_spinRA = nullptr;
|
||||
QDoubleSpinBox* m_spinRB = nullptr;
|
||||
QDoubleSpinBox* m_spinRC = nullptr;
|
||||
|
||||
// Middle
|
||||
QTreeWidget* m_treeObs = nullptr;
|
||||
QLabel* m_labelObsCount = nullptr;
|
||||
QPushButton* m_btnCapture = nullptr;
|
||||
QPushButton* m_btnLoadImages = nullptr;
|
||||
QPushButton* m_btnLoadPoses = nullptr;
|
||||
QPushButton* m_btnLoadDataset = nullptr;
|
||||
QPushButton* m_btnEditPose = nullptr;
|
||||
QPushButton* m_btnValidateAll = nullptr;
|
||||
QPushButton* m_btnExclude = nullptr;
|
||||
QPushButton* m_btnInclude = nullptr;
|
||||
QPushButton* m_btnDelete = nullptr;
|
||||
QPushButton* m_btnCalibrate = nullptr;
|
||||
QPushButton* m_btnCompare = nullptr;
|
||||
QPushButton* m_btnComputeErrors= nullptr;
|
||||
QPushButton* m_btnSave = nullptr;
|
||||
|
||||
QLabel* m_labelResult = nullptr;
|
||||
QProgressBar* m_progress = nullptr;
|
||||
QTextEdit* m_textLog = nullptr;
|
||||
|
||||
// Bottom
|
||||
QLabel* m_labelPreview = nullptr;
|
||||
QLabel* m_labelDataset = nullptr;
|
||||
|
||||
// 相机参数 (VisionGuide)
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> m_d;
|
||||
};
|
||||
Reference in New Issue
Block a user