first Commit

This commit is contained in:
lidg
2026-10-26 10:56:56 +08:00
commit c8cd533d9b
81 changed files with 128540 additions and 0 deletions
+279
View File
@@ -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;
};