Files
VisionGuide/source/MeasurePanel.cpp
T
2026-10-26 10:56:56 +08:00

1937 lines
87 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
// MeasurePanel.cpp
#include "MeasurePanel.h"
#include "ui_MainWindow.h"
#include "CameraPanel.h"
#include "LightPanel.h"
#include "YoloPanel.h"
#include "VisionGuidePanel.h"
#include "CoordTransformPanel.h"
#include "CameraManager.h"
#include "AppSettings.h"
#include <QCoreApplication>
#include <QElapsedTimer>
#include <QTimer>
#include <QFileInfo>
#include <QFile>
#include <QTextStream>
#include <QDateTime>
#include <QDir>
#include <QMenu>
#include <QAction>
#include <QFileDialog>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QPushButton>
#include <QMetaObject>
#include <QThread>
#include <QPointer>
#include <QEvent>
#include <QMouseEvent>
#include <QWheelEvent>
#include <QPainter>
#include <QFontMetrics>
#include <QRegExp>
#include <QResizeEvent>
#include <opencv2/opencv.hpp>
#include <algorithm>
#include <cmath>
// ── 中文步骤名常量 ──
#define S_CAPTURE "捕获图像"
#define S_LEFT_CAPTURE "左图原图"
#define S_RIGHT_CAPTURE "右图原图"
#define S_ROI "选择ROI"
#define S_LEFT_ROI "左图ROI"
#define S_RIGHT_ROI "右图ROI"
#define S_LEFT_ROI_POS "左图ROI位置"
#define S_RIGHT_ROI_POS "右图ROI位置"
#define S_YOLO "YOLO检测"
#define S_SEG "自动分割"
#define S_LEFT_SEG "左图自动分割"
#define S_RIGHT_SEG "右图自动分割"
#define S_POSE "位姿估计"
#define S_LEFT_POSE "左图位姿"
#define S_RIGHT_POSE "右图位姿"
#define S_FUSION "双目融合诊断"
#define S_GRASP_POSE "实际抓取点"
#define S_DEBUG "参数调试"
// ── 动态创建的右模板 UI 控件 ──
static QLabel* s_labelRightContourLibFile = nullptr;
static QPushButton* s_btnLoadRightContourLib = nullptr;
static QLabel* s_labelRightContourLibStatus = nullptr;
// “开始测量”的 10 s 是从按下按钮开始计算的全流程截止时间。
// 若左路已成功且剩余时间不足,不再启动可能耗时数秒的右路匹配。
static constexpr qint64 kMeasureDeadlineMs = 10000;
static constexpr qint64 kRightMatchReserveMs = 1500;
static constexpr qint64 kFastMeasureDeadlineMs = 5000;
static constexpr double kFastSlowLeftMs = 3000.0;
static cv::Mat qImageToGrayFast(const QImage& image)
{
if (image.isNull()) return cv::Mat();
if (image.format() == QImage::Format_Grayscale8) {
cv::Mat gray(image.height(), image.width(), CV_8UC1,
const_cast<uchar*>(image.constBits()), image.bytesPerLine());
return gray.clone();
}
if (image.format() == QImage::Format_RGB888) {
cv::Mat rgb(image.height(), image.width(), CV_8UC3,
const_cast<uchar*>(image.constBits()), image.bytesPerLine());
cv::Mat gray;
cv::cvtColor(rgb, gray, cv::COLOR_RGB2GRAY);
return gray;
}
const QImage rgbImage = image.convertToFormat(QImage::Format_RGB888);
cv::Mat rgb(rgbImage.height(), rgbImage.width(), CV_8UC3,
const_cast<uchar*>(rgbImage.constBits()), rgbImage.bytesPerLine());
cv::Mat gray;
cv::cvtColor(rgb, gray, cv::COLOR_RGB2GRAY);
return gray;
}
static QString poseLogLine(const QString& tag, const VisionPoseResult& p)
{
if (!p.valid)
return QStringLiteral(" %1 invalid: %2").arg(tag, p.errorMsg);
QString s = QStringLiteral(" %1 X=%2 Y=%3 Z=%4 mm Rx=%5 Ry=%6 Rz=%7 deg Score=%8 Time=%9ms")
.arg(tag)
.arg(p.tx * 1000.0, 0, 'f', 3)
.arg(p.ty * 1000.0, 0, 'f', 3)
.arg(p.tz * 1000.0, 0, 'f', 3)
.arg(p.rx, 0, 'f', 3)
.arg(p.ry, 0, 'f', 3)
.arg(p.rz, 0, 'f', 3)
.arg(p.score, 0, 'f', 4)
.arg(p.matchTimeMs, 0, 'f', 0);
s += QStringLiteral(" StdT=(%1,%2,%3)mm")
.arg(p.stdTx, 0, 'f', 3)
.arg(p.stdTy, 0, 'f', 3)
.arg(p.stdTz, 0, 'f', 3);
return s;
}
static QString poseShortLine(const QString& tag, const VisionPoseResult& p)
{
if (!p.valid)
return QStringLiteral("%1: invalid %2").arg(tag, p.errorMsg);
return QStringLiteral("%1: X=%2 Y=%3 Z=%4 mm Rx=%5 Ry=%6 Rz=%7 Score=%8")
.arg(tag)
.arg(p.tx * 1000.0, 0, 'f', 3)
.arg(p.ty * 1000.0, 0, 'f', 3)
.arg(p.tz * 1000.0, 0, 'f', 3)
.arg(p.rx, 0, 'f', 2)
.arg(p.ry, 0, 'f', 2)
.arg(p.rz, 0, 'f', 2)
.arg(p.score, 0, 'f', 4);
}
static QString zh(const wchar_t* text)
{
return QString::fromWCharArray(text);
}
static QFont reportFont(int pointSize, bool bold = false)
{
QFont f(QStringLiteral("Microsoft YaHei"));
f.setPointSize(pointSize);
f.setBold(bold);
return f;
}
static void drawTextBox(QPainter& p, const QRect& rect, const QStringList& lines,
const QColor& border, const QColor& titleColor)
{
p.save();
p.setRenderHint(QPainter::Antialiasing, true);
p.setPen(Qt::NoPen);
p.setBrush(QColor(255, 255, 255, 238));
p.drawRoundedRect(rect, 8, 8);
p.setPen(QPen(border, 2));
p.setBrush(Qt::NoBrush);
p.drawRoundedRect(rect, 8, 8);
QFont f = reportFont(14);
p.setFont(f);
QFontMetrics fm(f);
int y = rect.top() + 28;
for (int i = 0; i < lines.size(); ++i) {
p.setPen(i == 0 ? titleColor : QColor(20, 29, 43));
p.drawText(rect.left() + 18, y, lines[i]);
y += fm.height() + 6;
}
p.restore();
}
static QImage drawRoiOverlayImage(const QImage& src, const QRect& roi, const QString& label)
{
if (src.isNull())
return QImage();
QImage out = src.convertToFormat(QImage::Format_ARGB32);
QPainter p(&out);
p.setRenderHint(QPainter::Antialiasing, true);
const int penW = qMax(3, out.width() / 1500);
p.setPen(QPen(QColor(255, 204, 0), penW));
p.setBrush(Qt::NoBrush);
p.drawRect(roi.adjusted(0, 0, -1, -1));
QFont f = p.font();
f.setPointSize(qMax(18, out.width() / 170));
f.setBold(true);
p.setFont(f);
const QFontMetrics fm(f);
QRect textRect = fm.boundingRect(label).adjusted(-14, -10, 14, 10);
textRect.moveTopLeft(QPoint(roi.left(), qMax(0, roi.top() - textRect.height() - 8)));
p.fillRect(textRect, QColor(10, 18, 28, 185));
p.setPen(QColor(255, 230, 120));
p.drawText(textRect, Qt::AlignCenter, label);
return out;
}
static QImage drawPoseOverlayImage(VisionGuideMatcher* matcher, const QImage& src, const VisionPoseResult& pose)
{
if (!matcher || src.isNull() || !pose.valid)
return QImage();
cv::Mat vis = VisionGuideMatcher::qImageToMat(src);
if (vis.empty())
return QImage();
matcher->drawPoseResult(vis, pose);
return VisionGuideMatcher::matToQImage(vis);
}
static QImage makeFusionDiagnosticImage(const VisionPoseResult& leftPose,
const VisionPoseResult& rightPose,
const StereoPoseFusion::FusionResult& fusion,
const VisionPoseResult& finalPose,
qint64 totalMs)
{
QImage img(1600, 900, QImage::Format_ARGB32);
img.fill(QColor(248, 250, 252));
QPainter p(&img);
p.setRenderHint(QPainter::Antialiasing, true);
QFont titleFont = reportFont(30, true);
p.setFont(titleFont);
p.setPen(QColor(18, 28, 42));
p.drawText(QRect(56, 36, 1488, 58), Qt::AlignLeft | Qt::AlignVCenter,
zh(L"\x53cc\x76ee\x878d\x5408\x8bca\x65ad"));
QFont subFont = reportFont(15);
p.setFont(subFont);
p.setPen(QColor(86, 98, 115));
p.drawText(QRect(58, 92, 1200, 34), Qt::AlignLeft | Qt::AlignVCenter,
QDateTime::currentDateTime().toString(QStringLiteral("yyyy-MM-dd HH:mm:ss.zzz")));
drawTextBox(p, QRect(60, 150, 700, 210),
QStringList()
<< zh(L"\x5de6\x76f8\x673a\x539f\x59cb\x4f4d\x59ff")
<< poseShortLine(QStringLiteral("LEFT"), leftPose)
<< QStringLiteral("StdT=(%1,%2,%3) mm")
.arg(leftPose.stdTx, 0, 'f', 3)
.arg(leftPose.stdTy, 0, 'f', 3)
.arg(leftPose.stdTz, 0, 'f', 3)
<< (zh(L"\x8017\x65f6") + QStringLiteral("=%1 ms").arg(leftPose.matchTimeMs, 0, 'f', 0)),
QColor(60, 132, 246), QColor(37, 99, 235));
drawTextBox(p, QRect(840, 150, 700, 210),
QStringList()
<< zh(L"\x53f3\x76f8\x673a\x539f\x59cb\x4f4d\x59ff")
<< poseShortLine(QStringLiteral("RIGHT"), rightPose)
<< QStringLiteral("StdT=(%1,%2,%3) mm")
.arg(rightPose.stdTx, 0, 'f', 3)
.arg(rightPose.stdTy, 0, 'f', 3)
.arg(rightPose.stdTz, 0, 'f', 3)
<< (zh(L"\x8017\x65f6") + QStringLiteral("=%1 ms").arg(rightPose.matchTimeMs, 0, 'f', 0)),
QColor(16, 185, 129), QColor(5, 150, 105));
const QColor statusColor = fusion.consistent ? QColor(22, 163, 74) : QColor(220, 38, 38);
drawTextBox(p, QRect(60, 410, 700, 260),
QStringList()
<< zh(L"\x53f3\x76f8\x673a\x8f6c\x6362\x5230\x5de6\x76f8\x673a\x5750\x6807\x7cfb")
<< poseShortLine(QStringLiteral("RIGHT_IN_LEFT"), fusion.rightInLeft)
<< (zh(L"\x5de6\x53f3\x4f4d\x7f6e\x5dee") + QStringLiteral(" dt=%1 mm").arg(fusion.positionDisparityMm, 0, 'f', 3))
<< (zh(L"\x5de6\x53f3\x89d2\x5ea6\x5dee") + QStringLiteral(" daxis=%1 deg").arg(fusion.orientationDisparityDeg, 0, 'f', 3))
<< (zh(L"\x4e00\x81f4\x6027") + QStringLiteral("=%1").arg(fusion.consistent ? zh(L"\x901a\x8fc7") : zh(L"\x672a\x901a\x8fc7"))),
statusColor, statusColor);
drawTextBox(p, QRect(840, 410, 700, 260),
QStringList()
<< zh(L"\x6700\x7ec8\x8f93\x51fa")
<< poseShortLine(QStringLiteral("FINAL"), finalPose)
<< (zh(L"\x878d\x5408\x6743\x91cd") + QStringLiteral(" L=%1 R=%2")
.arg(fusion.weightLeft, 0, 'f', 3)
.arg(fusion.weightRight, 0, 'f', 3))
<< (zh(L"\x7f6e\x4fe1\x5ea6") + QStringLiteral("=%1 ").arg(fusion.confidence, 0, 'f', 3) +
zh(L"\x603b\x8017\x65f6") + QStringLiteral("=%1 ms")
.arg(totalMs)),
QColor(109, 40, 217), QColor(91, 33, 182));
QRect bar(100, 740, 1400, 34);
p.setPen(Qt::NoPen);
p.setBrush(QColor(226, 232, 240));
p.drawRoundedRect(bar, 17, 17);
const double ratio = qBound(0.0, fusion.positionDisparityMm / 5.0, 1.0);
QRect fill = bar;
fill.setWidth(qMax(18, int(bar.width() * ratio)));
p.setBrush(statusColor);
p.drawRoundedRect(fill, 17, 17);
p.setPen(QColor(18, 28, 42));
QFont labelFont = reportFont(17, true);
p.setFont(labelFont);
p.drawText(QRect(100, 790, 1400, 40), Qt::AlignCenter,
zh(L"\x878d\x5408\x95e8\x69db\x003a\x0020\x0064\x0074\x0020\x003c\x003d\x0020\x0035\x002e\x0030\x0020\x006d\x006d\x002c\x0020\x0064\x0061\x0078\x0069\x0073\x0020\x003c\x003d\x0020\x0031\x0030\x002e\x0030\x0020\x0064\x0065\x0067\x3002\x5f53\x524d\x0020\x0064\x0074\x003d") +
QString::number(fusion.positionDisparityMm, 'f', 3) +
zh(L"\x0020\x006d\x006d\x002c\x0020\x0064\x0061\x0078\x0069\x0073\x003d") +
QString::number(fusion.orientationDisparityDeg, 'f', 3) +
zh(L"\x0020\x0064\x0065\x0067\xff0c") +
(fusion.consistent ? zh(L"\x5141\x8bb8\x878d\x5408") : zh(L"\x62d2\x7edd\x878d\x5408\xff0c\x91c7\x7528\x53ef\x4fe1\x5355\x4fa7\x7ed3\x679c")));
return img;
}
MeasurePanel::MeasurePanel(Ui::MainWindowClass& u,
CameraPanel* cam, LightPanel* light, YoloPanel* yolo,
VisionGuidePanel* VisionGuide, CoordTransformPanel* coord,
VisionGuideMatcher* halM, StereoPoseFusion* fusion, QObject* parent)
: QObject(parent), ui(u),
m_cam(cam), m_light(light), m_yolo(yolo), m_visionGuide(VisionGuide),
m_coord(coord), m_visionMatcher(halM), m_fusion(fusion)
{
// ── 数据管理 ──
connect(ui.btnLoadLeftCalFile, &QPushButton::clicked, this, &MeasurePanel::onLoadLeftCal);
connect(ui.btnLoadRightCalFile, &QPushButton::clicked, this, &MeasurePanel::onLoadRightCal);
connect(ui.btnLoadStereoCal, &QPushButton::clicked, this, &MeasurePanel::onLoadStereoCal);
// 左模板 — 使用原有按钮
ui.btnLoadContourLib->setText(QString::fromUtf8("加载左模板..."));
ui.labelContourLibFile->setText(QString::fromUtf8("左: 未加载"));
connect(ui.btnLoadContourLib, &QPushButton::clicked, this, &MeasurePanel::onLoadShapeModelLeft);
// 右模板 — 动态添加控件到 groupContourLib 的 layout
{
auto* layout = qobject_cast<QVBoxLayout*>(ui.groupContourLib->layout());
if (layout) {
auto* rightRow = new QHBoxLayout;
s_labelRightContourLibFile = new QLabel(QString::fromUtf8("右: 未加载"));
s_btnLoadRightContourLib = new QPushButton(QString::fromUtf8("加载右模板..."));
rightRow->addWidget(s_labelRightContourLibFile);
rightRow->addWidget(s_btnLoadRightContourLib);
layout->insertLayout(layout->count() - 1, rightRow);
s_labelRightContourLibStatus = new QLabel(QString::fromUtf8("状态: 未加载"));
s_labelRightContourLibStatus->setStyleSheet("color: gray; font-size: 11px;");
layout->insertWidget(layout->count() - 1, s_labelRightContourLibStatus);
connect(s_btnLoadRightContourLib, &QPushButton::clicked,
this, &MeasurePanel::onLoadShapeModelRight);
}
}
ui.groupContourLib->setTitle(QString::fromUtf8("3D形状模板 (.sm3) — 左右分开"));
connect(ui.btnCapture, &QPushButton::clicked, this, &MeasurePanel::onCapture);
connect(ui.btnStartMeasure, &QPushButton::clicked, this, &MeasurePanel::onStartMeasure);
connect(m_coord, &CoordTransformPanel::poseDisplayRequested,
this, &MeasurePanel::onPoseDisplayRequested);
connect(ui.comboResultView, QOverload<int>::of(&QComboBox::currentIndexChanged),
this, &MeasurePanel::onResultViewChanged);
connect(ui.btnBrowseExportDir, &QPushButton::clicked, this, &MeasurePanel::onBrowseExportDir);
connect(ui.btnExportPoseTxt, &QPushButton::clicked, this, &MeasurePanel::onExportPoseCamera);
connect(ui.btnExportPoseCsv, &QPushButton::clicked, this, &MeasurePanel::onExportPoseRobot);
{
m_btnExportResultImages = new QPushButton(QString::fromUtf8("导出结果图(JPG)"), ui.groupExportPose);
m_btnExportResultImages->setEnabled(false);
if (auto* exportLayout = qobject_cast<QVBoxLayout*>(ui.groupExportPose->layout())) {
exportLayout->insertWidget(1, m_btnExportResultImages);
}
connect(m_btnExportResultImages, &QPushButton::clicked,
this, &MeasurePanel::onExportResultImages);
}
ui.labelROIResult->installEventFilter(this);
ui.labelROIResult->setMouseTracking(true);
ui.labelROIResult->setContextMenuPolicy(Qt::CustomContextMenu);
connect(ui.labelROIResult, &QWidget::customContextMenuRequested,
this, &MeasurePanel::onROIContextMenu);
// YOLO ROI 变化 → 同步到 VisionGuidePanel
connect(m_yolo, &YoloPanel::roiChanged, this, [this](const QRect& r){
m_visionGuide->setROI(r);
m_visionGuide->updateStepButtons();
if (r.isValid() && !m_capturedFrame.isNull()) {
storeResultImage(S_ROI, m_capturedFrame.copy(r));
ui.comboResultView->setCurrentText(S_ROI);
showCurrentResultImage();
}
});
connect(m_visionGuide, &VisionGuidePanel::resultImage, this, [this](const QString& n, const QImage& img){
storeResultImage(n, img);
ui.comboResultView->setCurrentText(n);
showCurrentResultImage();
});
connect(m_visionGuide, &VisionGuidePanel::poseReady, this, [this](const VisionPoseResult& p){
m_lastPose = p;
m_displayPose = p;
rememberPoseOverlaySource(m_capturedFrame, p, m_capturedSide);
displayPoseResult(p);
m_coord->setCameraPose(p);
});
// 初始化
ui.comboResultView->clear();
ui.comboResultView->addItem(S_CAPTURE);
ui.comboResultView->addItem(S_ROI);
m_exportDir = AppSettings::instance().get(AppSettings::K_ExportDir,
QCoreApplication::applicationDirPath() + "/export");
ui.editExportDir->setText(m_exportDir);
tryLoadDefaultStereoCalib();
// ★ 延迟加载轮廓库 (窗口先显示)
QTimer::singleShot(0, this, &MeasurePanel::autoLoadShapeModels);
QTimer::singleShot(1, this, [this]{ updateActionButtons(); });
updateActionButtons();
}
MeasurePanel::~MeasurePanel()
{
if (m_measureThread) {
m_measureThread->quit();
m_measureThread->wait(3000);
}
}
// ── 自动加载轮廓库 ──────────────────────────────────────────
void MeasurePanel::updateActionButtons()
{
const bool leftConnected = m_cam && m_cam->leftConnected();
const bool rightConnected = m_cam && m_cam->rightConnected();
const bool leftReady = leftConnected && m_visionMatcher && m_visionMatcher->isReady(CAM_LEFT);
const bool rightReady = rightConnected && m_visionMatcher && m_visionMatcher->isReady(CAM_RIGHT);
ui.btnCapture->setEnabled(!m_measureBusy && (leftConnected || rightConnected));
ui.btnStartMeasure->setEnabled(!m_measureBusy && (leftReady || rightReady));
if (m_btnExportResultImages)
m_btnExportResultImages->setEnabled(!m_measureBusy && m_resultImagesReadyForExport && !m_resultImages.isEmpty());
}
bool MeasurePanel::beginExternalMeasurement(const MeasurementCaptureSnapshot& snapshot)
{
if (m_measureBusy) return false;
++m_measureRunId; // Invalidate timers belonging to completed legacy measurements.
m_measureTimedOut = false;
m_measurePartialLeft = VisionPoseResult();
m_measurePartialRight = VisionPoseResult();
m_measureBusy = true;
m_lastPose = VisionPoseResult();
m_captureSnapshot = snapshot;
m_captureSnapshot.valid = false;
m_lastFusion = StereoPoseFusion::FusionResult();
m_poseOverlayBaseImage = QImage();
m_poseOverlayCenterPose = VisionPoseResult();
m_coord->setCameraPose(VisionPoseResult());
m_resultImages.clear();
m_resultImagesReadyForExport = false;
updateActionButtons();
emit measurementCaptureStarted();
return true;
}
void MeasurePanel::acceptExternalMeasurement(const MeasurementCaptureSnapshot& snapshot)
{
m_captureSnapshot = snapshot;
m_lastPose = snapshot.finalPose;
m_lastFusion = snapshot.fusion;
// The new tab computes robot targets with its verified calibration rule.
// Do not populate the legacy robot conversion with a differently-defined ABC.
if (snapshot.valid) displayPoseResult(snapshot.finalPose);
}
void MeasurePanel::endExternalMeasurement()
{
m_measureBusy = false;
updateActionButtons();
}
void MeasurePanel::autoLoadShapeModels()
{
auto& s = AppSettings::instance();
// 左模板
QString sm3Left = s.get(AppSettings::K_ShapeModelLeft, "");
if (sm3Left.isEmpty()) sm3Left = s.get(AppSettings::K_ShapeModel, ""); // 兼容旧版
if (!sm3Left.isEmpty() && QFile::exists(sm3Left)) {
if (m_visionMatcher->loadShapeModel(sm3Left, CAM_LEFT)) {
m_shapeModelFileLeft = sm3Left;
ui.labelContourLibFile->setText(QString::fromUtf8("左: %1").arg(QFileInfo(sm3Left).fileName()));
ui.labelContourLibStatus->setText(QString::fromUtf8("左模板已加载"));
ui.labelContourLibStatus->setStyleSheet("color:#00cc66;");
}
}
// 右模板
QString sm3Right = s.get(AppSettings::K_ShapeModelRight, "");
if (!sm3Right.isEmpty() && QFile::exists(sm3Right)) {
if (m_visionMatcher->loadShapeModel(sm3Right, CAM_RIGHT)) {
m_shapeModelFileRight = sm3Right;
if (s_labelRightContourLibFile)
s_labelRightContourLibFile->setText(QString::fromUtf8("右: %1").arg(QFileInfo(sm3Right).fileName()));
if (s_labelRightContourLibStatus) {
s_labelRightContourLibStatus->setText(QString::fromUtf8("右模板已加载: %1").arg(QFileInfo(sm3Right).fileName()));
s_labelRightContourLibStatus->setStyleSheet("color:#00cc66;");
}
}
}
}
// ── 默认路径 ─────────────────────────────────────────────────
QString MeasurePanel::defaultStereoCalibDir() {
QString d = QCoreApplication::applicationDirPath() + "/StereoCalib";
return QDir(d).exists() ? d : "D:/Qt/firstdemo/firstdemo/StereoCalib";
}
QString MeasurePanel::defaultLeftCalFile() { return defaultStereoCalibDir() + "/cam_left.cal"; }
QString MeasurePanel::defaultRightCalFile() { return defaultStereoCalibDir() + "/cam_right.cal"; }
QString MeasurePanel::defaultRelPoseFile() { return defaultStereoCalibDir() + "/pos_right2left.dat"; }
void MeasurePanel::tryLoadDefaultStereoCalib()
{
auto& s = AppSettings::instance();
QString lc = s.get(AppSettings::K_LeftCal, defaultLeftCalFile());
QString rc = s.get(AppSettings::K_RightCal, defaultRightCalFile());
QString rp = s.get(AppSettings::K_StereoCalDir, defaultRelPoseFile());
applyStereoCalibFiles(lc, rc, rp);
}
void MeasurePanel::applyStereoCalibFiles(const QString& l, const QString& r, const QString& rel)
{
auto& s = AppSettings::instance();
if (!l.isEmpty() && QFile::exists(l)) {
if (m_visionMatcher->loadCameraParams(l, CAM_LEFT)) {
m_leftCalFile = l;
ui.labelLeftCalFile->setText(QFileInfo(l).fileName());
s.set(AppSettings::K_LeftCal, l);
}
}
if (!r.isEmpty() && QFile::exists(r)) {
if (m_visionMatcher->loadCameraParams(r, CAM_RIGHT)) {
m_rightCalFile = r;
ui.labelRightCalFile->setText(QFileInfo(r).fileName());
s.set(AppSettings::K_RightCal, r);
}
}
if (!rel.isEmpty() && QFile::exists(rel)) {
m_stereoCalFile = rel;
if (ui.labelStereoCalFile)
ui.labelStereoCalFile->setText(QFileInfo(rel).fileName());
if (m_fusion) m_fusion->loadRightToLeftPose(rel);
s.set(AppSettings::K_StereoCalDir, rel);
}
}
// ── 数据管理 slots ───────────────────────────────────────────
void MeasurePanel::onLoadLeftCal() {
QString p = AppSettings::instance().openFile(
qobject_cast<QWidget*>(parent()), AppSettings::K_LeftCal,
QString::fromUtf8("左相机标定文件"), "VisionGuide Cal (*.cal);;All (*)",
defaultLeftCalFile());
if (p.isEmpty()) return;
m_leftCalFile = p;
ui.labelLeftCalFile->setText(QFileInfo(p).fileName());
if (m_visionMatcher->loadCameraParams(p, CAM_LEFT))
emit logMessage(QString::fromUtf8("左相机标定参数加载: ") + p);
m_visionGuide->updateStepButtons();
updateActionButtons();
}
void MeasurePanel::onLoadRightCal() {
QString p = AppSettings::instance().openFile(
qobject_cast<QWidget*>(parent()), AppSettings::K_RightCal,
QString::fromUtf8("右相机标定文件"), "VisionGuide Cal (*.cal);;All (*)",
defaultRightCalFile());
if (p.isEmpty()) return;
m_rightCalFile = p;
ui.labelRightCalFile->setText(QFileInfo(p).fileName());
if (m_visionMatcher->loadCameraParams(p, CAM_RIGHT))
emit logMessage(QString::fromUtf8("右相机标定参数加载: ") + p);
m_visionGuide->updateStepButtons();
updateActionButtons();
}
void MeasurePanel::onLoadStereoCal() {
QString p = AppSettings::instance().openFile(
qobject_cast<QWidget*>(parent()), AppSettings::K_StereoCalDir,
QString::fromUtf8("导入双目标定结果"),
"VisionGuide Cal/Pose (*.cal *.dat);;All (*)", defaultRelPoseFile());
if (p.isEmpty()) return;
QString dir = QFileInfo(p).absolutePath();
applyStereoCalibFiles(dir+"/cam_left.cal", dir+"/cam_right.cal", dir+"/pos_right2left.dat");
emit logMessage(QString::fromUtf8("双目标定结果已导入: ") + dir);
m_visionGuide->updateStepButtons();
updateActionButtons();
}
void MeasurePanel::onLoadShapeModelLeft() {
QString p = AppSettings::instance().openFile(
qobject_cast<QWidget*>(parent()), AppSettings::K_ShapeModelLeft,
QString::fromUtf8("加载左相机3D形状模板"), "VisionGuide SM3 (*.sm3);;All (*)");
if (p.isEmpty()) return;
m_shapeModelFileLeft = p;
if (m_visionMatcher->loadShapeModel(p, CAM_LEFT)) {
ui.labelContourLibFile->setText(QString::fromUtf8("左: %1").arg(QFileInfo(p).fileName()));
ui.labelContourLibStatus->setText(QString::fromUtf8("左模板已加载: %1").arg(QFileInfo(p).fileName()));
ui.labelContourLibStatus->setStyleSheet("color:#00cc66;");
} else {
ui.labelContourLibStatus->setText(QString::fromUtf8("左模板加载失败"));
ui.labelContourLibStatus->setStyleSheet("color:red;");
}
m_visionGuide->updateStepButtons();
updateActionButtons();
}
void MeasurePanel::onLoadShapeModelRight() {
QString p = AppSettings::instance().openFile(
qobject_cast<QWidget*>(parent()), AppSettings::K_ShapeModelRight,
QString::fromUtf8("加载右相机3D形状模板"), "VisionGuide SM3 (*.sm3);;All (*)");
if (p.isEmpty()) return;
m_shapeModelFileRight = p;
if (m_visionMatcher->loadShapeModel(p, CAM_RIGHT)) {
if (s_labelRightContourLibFile)
s_labelRightContourLibFile->setText(QString::fromUtf8("右: %1").arg(QFileInfo(p).fileName()));
if (s_labelRightContourLibStatus) {
s_labelRightContourLibStatus->setText(QString::fromUtf8("右模板已加载: %1").arg(QFileInfo(p).fileName()));
s_labelRightContourLibStatus->setStyleSheet("color:#00cc66;");
}
} else {
if (s_labelRightContourLibStatus) {
s_labelRightContourLibStatus->setText(QString::fromUtf8("右模板加载失败"));
s_labelRightContourLibStatus->setStyleSheet("color:red;");
}
}
m_visionGuide->updateStepButtons();
updateActionButtons();
}
// ── 采集 ─────────────────────────────────────────────────────
void MeasurePanel::onCapture()
{
emit logMessage(QString::fromUtf8("正在捕获图像..."));
CameraSide captureSide = CAM_LEFT;
m_capturedFrame = m_cam->captureLeft();
if (m_capturedFrame.isNull()) {
m_capturedFrame = m_cam->captureRight();
captureSide = CAM_RIGHT;
}
if (m_capturedFrame.isNull()) { emit logMessage(QString::fromUtf8("捕获失败")); return; }
m_capturedSide = captureSide;
emit logMessage(QString::fromUtf8("捕获成功: %1x%2")
.arg(m_capturedFrame.width()).arg(m_capturedFrame.height()));
storeResultImage(S_CAPTURE, m_capturedFrame);
ui.comboResultView->setCurrentIndex(0);
showCurrentResultImage();
m_yolo->setCapturedFrame(m_capturedFrame);
m_yolo->clearState();
m_visionGuide->setFrame(m_capturedFrame, captureSide);
m_visionGuide->setROI(QRect());
m_visionGuide->updateStepButtons();
updateActionButtons();
m_lastPose = VisionPoseResult();
m_displayPose = VisionPoseResult();
m_lastFusion = StereoPoseFusion::FusionResult();
m_poseOverlayBaseImage = QImage();
m_poseOverlayCenterPose = VisionPoseResult();
m_coord->setCameraPose(VisionPoseResult());
m_resultImagesReadyForExport = false;
m_resultImages.remove(S_YOLO); m_resultImages.remove(S_ROI);
m_resultImages.remove(S_LEFT_CAPTURE); m_resultImages.remove(S_RIGHT_CAPTURE);
m_resultImages.remove(S_LEFT_ROI); m_resultImages.remove(S_RIGHT_ROI);
m_resultImages.remove(S_LEFT_ROI_POS); m_resultImages.remove(S_RIGHT_ROI_POS);
m_resultImages.remove(S_LEFT_SEG); m_resultImages.remove(S_RIGHT_SEG);
m_resultImages.remove(S_LEFT_POSE); m_resultImages.remove(S_RIGHT_POSE);
m_resultImages.remove(S_FUSION);
m_resultImages.remove(S_SEG); m_resultImages.remove(S_POSE);
m_resultImages.remove(S_GRASP_POSE);
m_resultImages.remove(S_DEBUG);
ui.editResultX->clear(); ui.editResultY->clear(); ui.editResultZ->clear();
ui.editResultAxis->clear(); ui.editResultClass->clear(); ui.editResultTime->clear();
ui.groupResult->setTitle(QString::fromUtf8("位姿估计结果"));
ui.textAlgoResult->clear(); ui.progressAlgoStep->setValue(0);
}
// ── 全流程测量 ───────────────────────────────────────────────
void MeasurePanel::onStartMeasure()
{
{
if (m_measureBusy) {
emit logMessage(QStringLiteral("[Measure] Previous measurement is still running; request ignored."));
emit measureCompleted(false);
return;
}
const bool leftReady = m_cam && m_cam->leftConnected() && m_visionMatcher && m_visionMatcher->isReady(CAM_LEFT);
m_captureSnapshot.valid = false;
const bool rightReady = m_cam && m_cam->rightConnected() && m_visionMatcher && m_visionMatcher->isReady(CAM_RIGHT);
const bool dualMode = leftReady && rightReady && m_fusion && m_fusion->isReady();
// “开始测量”前再次读取“位姿估计”页参数,保证自动分割开关即时生效。
if (m_visionGuide)
m_visionGuide->applyCurrentParameters();
emit logMessage(dualMode
? QStringLiteral("=== Start stereo measurement ===")
: QStringLiteral("=== Start single-camera measurement ==="));
emit logMessage(m_visionMatcher && m_visionMatcher->params.useAutoSeg
? QString::fromUtf8("[Measure] 自动分割=启用:YOLO ROI → 自动分割 → 3D形状匹配。")
: QString::fromUtf8("[Measure] 自动分割=关闭:YOLO ROI → 直接3D形状匹配。"));
if (!leftReady && !rightReady) {
updateActionButtons();
emit logMessage(QStringLiteral("[Measure] No ready camera path. Check camera connection, calibration and 3D model."));
emit measureCompleted(false);
return;
}
m_measureBusy = true;
m_measureTimedOut = false;
const quint64 runId = ++m_measureRunId;
m_measurePartialLeft = VisionPoseResult();
m_measurePartialRight = VisionPoseResult();
m_resultImagesReadyForExport = false;
updateActionButtons();
ui.progressAlgo->setValue(0);
m_lastPose = VisionPoseResult();
m_displayPose = VisionPoseResult();
m_lastFusion = StereoPoseFusion::FusionResult();
m_poseOverlayBaseImage = QImage();
m_poseOverlayCenterPose = VisionPoseResult();
m_coord->setCameraPose(VisionPoseResult());
m_resultImages.remove(S_LEFT_CAPTURE); m_resultImages.remove(S_RIGHT_CAPTURE);
m_resultImages.remove(S_LEFT_ROI); m_resultImages.remove(S_RIGHT_ROI);
m_resultImages.remove(S_LEFT_ROI_POS); m_resultImages.remove(S_RIGHT_ROI_POS);
m_resultImages.remove(S_LEFT_SEG); m_resultImages.remove(S_RIGHT_SEG);
m_resultImages.remove(S_LEFT_POSE); m_resultImages.remove(S_RIGHT_POSE);
m_resultImages.remove(S_FUSION);
m_resultImages.remove(S_POSE);
m_resultImages.remove(S_GRASP_POSE);
const qint64 startMs = QDateTime::currentMSecsSinceEpoch();
m_captureSnapshot = MeasurementCaptureSnapshot();
m_captureSnapshot.runId = runId;
m_captureSnapshot.captureBeginMs = startMs;
m_captureSnapshot.parameters = m_visionMatcher->params;
m_captureSnapshot.leftCal = m_visionMatcher->cameraParamFile(CAM_LEFT);
m_captureSnapshot.rightCal = m_visionMatcher->cameraParamFile(CAM_RIGHT);
m_captureSnapshot.leftModel = m_visionMatcher->shapeModelFile(CAM_LEFT);
m_captureSnapshot.rightModel = m_visionMatcher->shapeModelFile(CAM_RIGHT);
m_captureSnapshot.stereoPoseFile = m_stereoCalFile;
emit measurementCaptureStarted();
QImage leftImg;
QImage rightImg;
if (leftReady) {
ui.progressAlgo->setValue(10);
leftImg = m_cam->captureLeft();
if (leftImg.isNull())
emit logMessage(QStringLiteral(" LEFT capture failed."));
}
if (rightReady && (dualMode || !leftReady)) {
ui.progressAlgo->setValue(20);
rightImg = m_cam->captureRight();
if (rightImg.isNull())
emit logMessage(QStringLiteral(" RIGHT capture failed."));
}
if (leftImg.isNull() && rightImg.isNull()) {
emit logMessage(QStringLiteral("[Measure] No image captured."));
appendFusionCsv(VisionPoseResult(), VisionPoseResult(),
StereoPoseFusion::FusionResult(), VisionPoseResult(),
QDateTime::currentMSecsSinceEpoch() - startMs);
ui.progressAlgo->setValue(0);
m_measureBusy = false;
updateActionButtons();
emit measureCompleted(false);
return;
}
m_capturedFrame = leftImg.isNull() ? rightImg : leftImg;
m_captureSnapshot.leftImage = leftImg;
m_captureSnapshot.rightImage = rightImg;
m_captureSnapshot.captureEndMs = QDateTime::currentMSecsSinceEpoch();
storeResultImage(S_CAPTURE, m_capturedFrame);
if (!leftImg.isNull())
storeResultImage(S_LEFT_CAPTURE, leftImg);
if (!rightImg.isNull())
storeResultImage(S_RIGHT_CAPTURE, rightImg);
m_visionGuide->setFrame(m_capturedFrame, leftImg.isNull() ? CAM_RIGHT : CAM_LEFT);
m_visionGuide->setROI(QRect());
QRect roiL;
QRect roiR;
cv::Mat leftGray;
cv::Mat rightGray;
if (!leftImg.isNull() && leftReady) {
ui.progressAlgo->setValue(32);
roiL = m_yolo->runYoloAndCrop(leftImg, -1, {}, QStringLiteral("LEFT"));
if (roiL.isValid()) {
m_visionGuide->setFrame(leftImg, CAM_LEFT);
m_visionGuide->setROI(roiL);
storeResultImage(S_LEFT_ROI, leftImg.copy(roiL));
storeResultImage(S_LEFT_ROI_POS,
drawRoiOverlayImage(leftImg, roiL, QStringLiteral("LEFT ROI")));
QImage rgb = leftImg.convertToFormat(QImage::Format_RGB888);
cv::Mat rgbMat(rgb.height(), rgb.width(), CV_8UC3,
const_cast<uchar*>(rgb.constBits()), rgb.bytesPerLine());
cv::cvtColor(rgbMat, leftGray, cv::COLOR_RGB2GRAY);
} else {
emit logMessage(QStringLiteral(" LEFT ROI invalid; skip LEFT VisionGuide match."));
}
}
if (!rightImg.isNull() && rightReady) {
ui.progressAlgo->setValue(45);
roiR = m_yolo->runYoloAndCrop(rightImg, -1, roiL, QStringLiteral("RIGHT"));
if (roiR.isValid()) {
if (leftGray.empty()) {
m_visionGuide->setFrame(rightImg, CAM_RIGHT);
m_visionGuide->setROI(roiR);
}
storeResultImage(S_RIGHT_ROI, rightImg.copy(roiR));
storeResultImage(S_RIGHT_ROI_POS,
drawRoiOverlayImage(rightImg, roiR, QStringLiteral("RIGHT ROI")));
QImage rgb = rightImg.convertToFormat(QImage::Format_RGB888);
cv::Mat rgbMat(rgb.height(), rgb.width(), CV_8UC3,
const_cast<uchar*>(rgb.constBits()), rgb.bytesPerLine());
cv::cvtColor(rgbMat, rightGray, cv::COLOR_RGB2GRAY);
} else {
emit logMessage(QStringLiteral(" RIGHT ROI invalid; skip RIGHT VisionGuide match."));
}
}
if (leftGray.empty() && rightGray.empty()) {
emit logMessage(QStringLiteral("[Measure] No valid ROI for VisionGuide matching."));
appendFusionCsv(VisionPoseResult(), VisionPoseResult(),
StereoPoseFusion::FusionResult(), VisionPoseResult(),
QDateTime::currentMSecsSinceEpoch() - startMs);
ui.progressAlgo->setValue(0);
m_measureBusy = false;
updateActionButtons();
emit measureCompleted(false);
return;
}
ui.progressAlgo->setValue(60);
m_captureSnapshot.leftRoi = roiL;
m_captureSnapshot.rightRoi = roiR;
emit logMessage(QStringLiteral("[Measure] VisionGuide matching is running in background..."));
QPointer<MeasurePanel> self(this);
VisionGuideMatcher* matcher = m_visionMatcher;
StereoPoseFusion* fusionEngine = m_fusion;
QThread* thread = new QThread(this);
QObject* worker = new QObject;
worker->moveToThread(thread);
connect(thread, &QThread::finished, worker, &QObject::deleteLater);
connect(thread, &QThread::finished, thread, &QObject::deleteLater);
connect(thread, &QThread::finished, this, [this, thread, runId]() {
if (m_measureThread == thread)
m_measureThread = nullptr;
// 视觉引导 的同步匹配无法安全强制终止。超时结果已经返回后,
// 等后台调用自然退出再允许下一轮,避免两个测量并发访问同一 matcher。
if (m_measureRunId == runId && m_measureTimedOut) {
m_measureBusy = false;
updateActionButtons();
emit logMessage(QString::fromUtf8(
"[Measure] 超时测量的后台任务已结束,现在可以开始下一次测量。"));
}
});
connect(thread, &QThread::started, worker,
[self, matcher, fusionEngine, leftImg, rightImg, leftGray, rightGray, roiL, roiR,
leftReady, rightReady, dualMode, startMs, runId]() mutable {
VisionPoseResult poseL;
VisionPoseResult poseR;
StereoPoseFusion::FusionResult fusion;
VisionPoseResult finalPose;
bool rightSkippedForDeadline = false;
try {
if (leftReady && !leftGray.empty() && roiL.isValid()) {
poseL = matcher->match(leftGray, roiL, CAM_LEFT);
if (self) {
QMetaObject::invokeMethod(self.data(), [self, runId, poseL]() {
if (self && self->m_measureRunId == runId && !self->m_measureTimedOut)
self->m_measurePartialLeft = poseL;
}, Qt::BlockingQueuedConnection);
}
}
if (rightReady && !rightGray.empty() && roiR.isValid()) {
const qint64 elapsedMs = QDateTime::currentMSecsSinceEpoch() - startMs;
if (poseL.valid && elapsedMs >= kMeasureDeadlineMs - kRightMatchReserveMs) {
rightSkippedForDeadline = true;
} else {
poseR = matcher->match(rightGray, roiR, CAM_RIGHT);
if (self) {
QMetaObject::invokeMethod(self.data(), [self, runId, poseR]() {
if (self && self->m_measureRunId == runId && !self->m_measureTimedOut)
self->m_measurePartialRight = poseR;
}, Qt::BlockingQueuedConnection);
}
}
}
if (dualMode && !rightSkippedForDeadline) {
fusion = fusionEngine->fuse(poseL, poseR, StereoPoseFusion::FM_InverseVariance,1.0,10.0,matcher->cameraParamFile(CAM_LEFT),matcher->cameraParamFile(CAM_RIGHT));
finalPose = fusion.fused;
} else {
finalPose = poseL.valid ? poseL : poseR;
}
} catch (...) {
finalPose.valid = false;
finalPose.errorMsg = QStringLiteral("Unhandled exception during background measurement.");
}
const qint64 totalMs = QDateTime::currentMSecsSinceEpoch() - startMs;
if (!self) {
QThread::currentThread()->quit();
return;
}
QMetaObject::invokeMethod(self.data(), [self, poseL, poseR, fusion, finalPose,
leftImg, rightImg, dualMode, totalMs,
rightSkippedForDeadline, runId]() mutable {
if (!self) return;
if (self->m_measureRunId != runId) {
emit self->logMessage(QStringLiteral("[Measure] Stale worker result discarded."));
return;
}
if (self->m_measureTimedOut) {
emit self->logMessage(QStringLiteral("[Measure] Worker finished after the 10 s timeout; result discarded."));
return;
}
self->m_lastFusion = fusion;
self->m_lastPose = finalPose;
self->m_captureSnapshot.leftPose = poseL;
self->m_captureSnapshot.rightPose = poseR;
self->m_captureSnapshot.finalPose = finalPose;
self->m_captureSnapshot.fusion = fusion;
self->m_captureSnapshot.finalSide = (dualMode && !rightSkippedForDeadline) || poseL.valid ? CAM_LEFT : CAM_RIGHT;
self->m_captureSnapshot.valid = finalPose.valid;
self->m_displayPose = finalPose;
emit self->logMessage(poseLogLine(QStringLiteral("LEFT_RAW"), poseL));
emit self->logMessage(poseLogLine(QStringLiteral("RIGHT_RAW"), poseR));
if (rightSkippedForDeadline) {
emit self->logMessage(QString::fromUtf8(
"[Measure] 左路已成功且接近 10 s 截止时间,跳过右路匹配并返回左路结果。"));
}
if (dualMode && !rightSkippedForDeadline) {
emit self->logMessage(QStringLiteral(" ") + fusion.message);
emit self->logMessage(poseLogLine(QStringLiteral("LEFT_IN_LEFT"), fusion.leftInLeft));
emit self->logMessage(poseLogLine(QStringLiteral("RIGHT_IN_LEFT"), fusion.rightInLeft));
emit self->logMessage(QStringLiteral(" FUSION_METRIC dt=%1mm daxis=%2deg consistent=%3 wL=%4 wR=%5 confidence=%6")
.arg(fusion.positionDisparityMm, 0, 'f', 3)
.arg(fusion.orientationDisparityDeg, 0, 'f', 3)
.arg(fusion.consistent ? QStringLiteral("true") : QStringLiteral("false"))
.arg(fusion.weightLeft, 0, 'f', 3)
.arg(fusion.weightRight, 0, 'f', 3)
.arg(fusion.confidence, 0, 'f', 3));
emit self->logMessage(poseLogLine(QStringLiteral("FUSED_LEFT_FRAME"), fusion.fused));
}
if (!poseL.segmentationDebugImage.empty()) {
self->storeResultImage(S_LEFT_SEG,
VisionGuideMatcher::matToQImage(poseL.segmentationDebugImage));
}
if (!poseR.segmentationDebugImage.empty()) {
self->storeResultImage(S_RIGHT_SEG,
VisionGuideMatcher::matToQImage(poseR.segmentationDebugImage));
}
if (poseL.valid && !leftImg.isNull()) {
self->storeResultImage(S_LEFT_POSE,
drawPoseOverlayImage(self->m_visionMatcher, leftImg, poseL));
}
if (poseR.valid && !rightImg.isNull()) {
self->storeResultImage(S_RIGHT_POSE,
drawPoseOverlayImage(self->m_visionMatcher, rightImg, poseR));
}
self->storeResultImage(S_FUSION,
makeFusionDiagnosticImage(poseL, poseR, fusion, finalPose, totalMs));
if (finalPose.valid) {
emit self->logMessage(poseLogLine(QStringLiteral("FINAL_OUTPUT"), finalPose));
self->displayPoseResult(finalPose);
VisionPoseResult poseForDraw = finalPose;
bool drawRightRaw = false;
if (dualMode && poseR.valid && !rightImg.isNull()) {
drawRightRaw = !poseL.valid ||
(!fusion.consistent && fusion.weightRight > fusion.weightLeft);
}
QImage poseFrame = drawRightRaw ? rightImg
: ((dualMode || poseL.valid) ? leftImg : rightImg);
if (drawRightRaw) {
poseForDraw = poseR;
emit self->logMessage(QStringLiteral("[Measure] result view uses RIGHT image because stereo fusion selected right pose."));
}
if (poseFrame.isNull())
poseFrame = self->m_capturedFrame;
const bool poseUsesRightCamera = drawRightRaw ||
(leftImg.isNull() && !rightImg.isNull());
const CameraSide poseSide = poseUsesRightCamera ? CAM_RIGHT : CAM_LEFT;
self->m_visionMatcher->updatePoseProjection(poseForDraw, poseSide, true);
self->rememberPoseOverlaySource(poseFrame, poseForDraw, poseSide);
cv::Mat poseVis = VisionGuideMatcher::qImageToMat(poseFrame);
if (!poseVis.empty()) {
self->m_visionMatcher->drawPoseResult(poseVis, poseForDraw);
QImage finalImage = VisionGuideMatcher::matToQImage(poseVis);
self->storeResultImage(S_POSE, finalImage);
self->showExternalResultImage(S_POSE, finalImage);
self->ui.tabWidget->setCurrentWidget(self->ui.tabVisionAlgo);
}
self->ui.editResultTime->setText(QString::number(totalMs));
self->m_coord->setCameraPose(finalPose);
self->appendFusionCsv(poseL, poseR, fusion, finalPose, totalMs);
self->ui.progressAlgo->setValue(100);
emit self->logMessage(QStringLiteral("=== Measurement completed Score=%1 Total=%2ms ===")
.arg(finalPose.score, 0, 'f', 4)
.arg(totalMs));
} else {
emit self->logMessage(QStringLiteral("[Measure] Pose measurement failed: %1").arg(finalPose.errorMsg));
self->appendFusionCsv(poseL, poseR, fusion, finalPose, totalMs);
self->ui.progressAlgo->setValue(0);
}
self->m_resultImagesReadyForExport = true;
self->m_measureBusy = false;
self->updateActionButtons();
emit self->measureCompleted(finalPose.valid);
}, Qt::QueuedConnection);
QThread::currentThread()->quit();
}, Qt::QueuedConnection);
m_measureThread = thread;
thread->start();
// 旧实现的 singleShot 只检查共享 bool:上一轮的定时器可能在下一轮
// 测量期间触发,造成“只运行了 3 s 却报 10 s 超时”。runId 将定时器
// 与本轮测量绑定;remainingMs 则让 10 s 覆盖采图、YOLO 和匹配全流程。
const qint64 elapsedBeforeWorkerMs = QDateTime::currentMSecsSinceEpoch() - startMs;
const int remainingMs = static_cast<int>(
std::max<qint64>(0, kMeasureDeadlineMs - elapsedBeforeWorkerMs));
QTimer::singleShot(remainingMs, this,
[this, runId, startMs, leftImg, rightImg]() {
if (m_measureRunId != runId || !m_measureBusy || m_measureTimedOut)
return;
m_measureTimedOut = true;
const qint64 totalMs = QDateTime::currentMSecsSinceEpoch() - startMs;
const VisionPoseResult poseL = m_measurePartialLeft;
const VisionPoseResult poseR = m_measurePartialRight;
const bool useLeft = poseL.valid && (!poseR.valid || poseL.score >= poseR.score);
const VisionPoseResult fallbackPose = useLeft ? poseL : poseR;
if (fallbackPose.valid) {
m_lastFusion = StereoPoseFusion::FusionResult();
m_lastPose = fallbackPose;
m_captureSnapshot.leftPose = poseL;
m_captureSnapshot.rightPose = poseR;
m_captureSnapshot.finalPose = fallbackPose;
m_captureSnapshot.finalSide = useLeft ? CAM_LEFT : CAM_RIGHT;
m_captureSnapshot.valid = true;
m_displayPose = fallbackPose;
emit logMessage(QString::fromUtf8(
"[Measure][TIMEOUT] 全流程达到 %1 ms;%2路已经匹配成功,立即返回该路结果,不再等待另一相机。")
.arg(totalMs)
.arg(useLeft ? QString::fromUtf8("左") : QString::fromUtf8("右")));
emit logMessage(poseLogLine(QStringLiteral("LEFT_RAW_AT_TIMEOUT"), poseL));
emit logMessage(poseLogLine(QStringLiteral("RIGHT_RAW_AT_TIMEOUT"), poseR));
emit logMessage(poseLogLine(QStringLiteral("FINAL_OUTPUT_TIMEOUT_FALLBACK"), fallbackPose));
displayPoseResult(fallbackPose);
QImage poseFrame = useLeft ? leftImg : rightImg;
if (poseFrame.isNull())
poseFrame = m_capturedFrame;
VisionPoseResult poseForDraw = fallbackPose;
const CameraSide poseSide = useLeft ? CAM_LEFT : CAM_RIGHT;
m_visionMatcher->updatePoseProjection(poseForDraw, poseSide, true);
rememberPoseOverlaySource(poseFrame, poseForDraw, poseSide);
cv::Mat poseVis = VisionGuideMatcher::qImageToMat(poseFrame);
if (!poseVis.empty()) {
m_visionMatcher->drawPoseResult(poseVis, poseForDraw);
const QImage finalImage = VisionGuideMatcher::matToQImage(poseVis);
storeResultImage(S_POSE, finalImage);
showExternalResultImage(S_POSE, finalImage);
ui.tabWidget->setCurrentWidget(ui.tabVisionAlgo);
}
ui.editResultTime->setText(QString::number(totalMs));
m_coord->setCameraPose(fallbackPose);
appendFusionCsv(poseL, poseR, StereoPoseFusion::FusionResult(),
fallbackPose, totalMs);
ui.progressAlgo->setValue(100);
m_resultImagesReadyForExport = true;
emit logMessage(QString::fromUtf8(
"=== Measurement completed by single-camera timeout fallback Score=%1 Total=%2ms ===")
.arg(fallbackPose.score, 0, 'f', 4)
.arg(totalMs));
emit measureCompleted(true);
} else {
ui.progressAlgo->setValue(0);
emit logMessage(QString::fromUtf8(
"[Measure][ERROR] 全流程达到 %1 ms,左右路均无可用结果,已返回失败。")
.arg(totalMs));
emit measureCompleted(false);
}
// 不强杀正在执行的 视觉引导 调用,也不允许下一轮与其并发。
// 数值结果已经按时返回;后台退出后 finished 回调会重新启用按钮。
updateActionButtons();
});
return;
}
}
double MeasurePanel::currentGraspOffsetMm() const
{
return ui.spinGraspOffsetZMm->value();
}
VisionGuideMatcher::Params MeasurePanel::currentVisionParameters(){
if(m_visionGuide)m_visionGuide->applyCurrentParameters();
return m_visionMatcher?m_visionMatcher->params:VisionGuideMatcher::Params();
}
// ── 独立快速测量(5 s)────────────────────────────────────────
void MeasurePanel::onFastMeasure()
{
if (m_measureBusy) {
emit logMessage(QString::fromUtf8("[快速测量] 当前仍有测量任务运行,请等待其结束。"));
return;
}
const bool leftReady = m_cam && m_cam->leftConnected() &&
m_visionMatcher && m_visionMatcher->isReady(CAM_LEFT);
const bool rightReady = m_cam && m_cam->rightConnected() &&
m_visionMatcher && m_visionMatcher->isReady(CAM_RIGHT);
const bool stereoReady = rightReady && m_fusion && m_fusion->isReady();
if (!leftReady) {
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 左相机、左标定或左3D模板未就绪。"));
emit measureCompleted(false);
return;
}
if (m_visionGuide)
m_visionGuide->applyCurrentParameters();
m_measureBusy = true;
m_measureTimedOut = false;
const quint64 runId = ++m_measureRunId;
m_measurePartialLeft = VisionPoseResult();
m_measurePartialRight = VisionPoseResult();
m_resultImagesReadyForExport = false;
m_lastPose = VisionPoseResult();
m_lastFusion = StereoPoseFusion::FusionResult();
ui.progressAlgo->setValue(0);
ui.editResultX->clear(); ui.editResultY->clear(); ui.editResultZ->clear();
ui.editResultAxis->clear(); ui.editResultClass->clear(); ui.editResultTime->clear();
// 快速流程的图像结果严格只保留“原图”和“位姿估计”。
const QStringList measurementImages = {
S_CAPTURE, S_LEFT_CAPTURE, S_RIGHT_CAPTURE, S_ROI, S_LEFT_ROI, S_RIGHT_ROI,
S_LEFT_ROI_POS, S_RIGHT_ROI_POS, S_YOLO, S_SEG, S_LEFT_SEG, S_RIGHT_SEG,
S_LEFT_POSE, S_RIGHT_POSE, S_FUSION, S_DEBUG, S_POSE
};
for (const QString& name : measurementImages) {
m_resultImages.remove(name);
const int comboIndex = ui.comboResultView->findText(name);
if (comboIndex >= 0) ui.comboResultView->removeItem(comboIndex);
}
updateActionButtons();
const qint64 startMs = QDateTime::currentMSecsSinceEpoch();
emit logMessage(QString::fromUtf8(
"=== 快速测量开始(截止5秒;左图YOLO;右图使用双目标定预测ROI;跳过自动分割) ==="));
QElapsedTimer stage;
stage.start();
ui.progressAlgo->setValue(10);
const QImage leftImg = m_cam->captureLeft();
const qint64 captureLeftMs = stage.restart();
QImage rightImg;
qint64 captureRightMs = 0;
if (stereoReady) {
rightImg = m_cam->captureRight();
captureRightMs = stage.restart();
}
if (leftImg.isNull()) {
m_measureBusy = false;
updateActionButtons();
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 左图采集失败。"));
emit measureCompleted(false);
return;
}
m_capturedFrame = leftImg;
storeResultImage(S_CAPTURE, leftImg);
ui.progressAlgo->setValue(25);
stage.restart();
const QRect roiL = m_yolo->runYoloAndCrop(leftImg, -1, {}, QStringLiteral("FAST_LEFT"));
const qint64 yoloLeftMs = stage.elapsed();
if (!roiL.isValid()) {
m_measureBusy = false;
ui.progressAlgo->setValue(0);
updateActionButtons();
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 左图YOLO未得到有效ROI。"));
emit measureCompleted(false);
return;
}
stage.restart();
const cv::Mat leftGray = qImageToGrayFast(leftImg);
const qint64 grayLeftMs = stage.elapsed();
if (leftGray.empty()) {
m_measureBusy = false;
ui.progressAlgo->setValue(0);
updateActionButtons();
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 左图灰度转换失败。"));
emit measureCompleted(false);
return;
}
if (QDateTime::currentMSecsSinceEpoch() - startMs >= kFastMeasureDeadlineMs) {
m_measureBusy = false;
ui.progressAlgo->setValue(0);
updateActionButtons();
emit logMessage(QString::fromUtf8("[快速测量][ERROR] 采图和左图预处理已达到5秒,返回失败。"));
emit measureCompleted(false);
return;
}
ui.progressAlgo->setValue(45);
QPointer<MeasurePanel> self(this);
VisionGuideMatcher* matcher = m_visionMatcher;
StereoPoseFusion* fusionEngine = m_fusion;
const QString relPoseFile = stereoReady ? m_fusion->relPoseFile() : QString();
QThread* thread = new QThread(this);
QObject* worker = new QObject;
worker->moveToThread(thread);
connect(thread, &QThread::finished, worker, &QObject::deleteLater);
connect(thread, &QThread::finished, thread, &QObject::deleteLater);
connect(thread, &QThread::finished, this, [this, thread, runId]() {
if (m_measureThread == thread) m_measureThread = nullptr;
if (m_measureRunId == runId && m_measureTimedOut) {
m_measureBusy = false;
updateActionButtons();
emit logMessage(QString::fromUtf8(
"[快速测量] 超时后台任务已结束,现在可以再次测量。"));
}
});
connect(thread, &QThread::started, worker,
[self, matcher, fusionEngine, leftImg, rightImg, leftGray, roiL,
stereoReady, relPoseFile, startMs, runId,
captureLeftMs, captureRightMs, yoloLeftMs, grayLeftMs]() mutable {
VisionPoseResult poseL;
VisionPoseResult poseR;
StereoPoseFusion::FusionResult fusion;
VisionPoseResult finalPose;
QRect roiR;
bool rightAttempted = false;
bool rightSkippedByLeftRule = false;
QString decision;
qint64 leftMatchWallMs = 0;
qint64 rightRoiPredictMs = 0;
qint64 grayRightMs = 0;
qint64 rightMatchWallMs = 0;
qint64 fusionMs = 0;
try {
QElapsedTimer timer;
timer.start();
poseL = matcher->match(leftGray, roiL, CAM_LEFT, true);
leftMatchWallMs = timer.elapsed();
const bool slowAccepted = poseL.valid && poseL.matchTimeMs >= kFastSlowLeftMs &&
poseL.score > 0.80;
const bool fastAccepted = poseL.valid && poseL.matchTimeMs < kFastSlowLeftMs &&
poseL.score > 0.85;
rightSkippedByLeftRule = slowAccepted || fastAccepted;
if (rightSkippedByLeftRule) {
finalPose = poseL;
decision = slowAccepted
? QString::fromUtf8("左路匹配时间≥3秒且Score>0.80,跳过右路")
: QString::fromUtf8("左路匹配时间<3秒且Score>0.85,跳过右路");
} else if (stereoReady && !rightImg.isNull() &&
QDateTime::currentMSecsSinceEpoch() - startMs < kFastMeasureDeadlineMs) {
timer.restart();
const double estimatedDepth = poseL.valid && poseL.tz > 0.03 ? poseL.tz : 0.30;
const bool predicted = matcher->predictRightRoi(
roiL, relPoseFile, rightImg.size(), roiR, estimatedDepth, 0.25);
rightRoiPredictMs = timer.elapsed();
if (predicted) {
rightAttempted = true;
timer.restart();
const cv::Mat rightGray = qImageToGrayFast(rightImg);
grayRightMs = timer.elapsed();
if (!rightGray.empty()) {
timer.restart();
poseR = matcher->match(rightGray, roiR, CAM_RIGHT, true);
rightMatchWallMs = timer.elapsed();
} else {
poseR.errorMsg = QString::fromUtf8("右图灰度转换失败");
}
decision = QString::fromUtf8("左路未达到快速接受阈值,已使用双目标定预测右ROI并计算右路");
} else {
decision = QString::fromUtf8("右ROI双目标定投影失败,未执行右图完整YOLO");
finalPose = stereoReady ? VisionPoseResult() : poseL;
if (!finalPose.valid)
finalPose.errorMsg = QString::fromUtf8("右ROI预测失败");
}
} else {
finalPose = poseL;
decision = stereoReady
? QString::fromUtf8("剩余时间不足,未启动右路")
: QString::fromUtf8("右路未就绪,使用左路");
}
if (rightAttempted) {
timer.restart();
fusion = fusionEngine->fuse(poseL, poseR, StereoPoseFusion::FM_InverseVariance,1.0,10.0,matcher->cameraParamFile(CAM_LEFT),matcher->cameraParamFile(CAM_RIGHT));
fusionMs = timer.elapsed();
finalPose = fusion.fused;
} else if (!finalPose.valid && poseL.valid && !stereoReady) {
finalPose = poseL;
}
} catch (...) {
finalPose = VisionPoseResult();
finalPose.errorMsg = QString::fromUtf8("快速测量后台发生未处理异常");
}
const qint64 totalMs = QDateTime::currentMSecsSinceEpoch() - startMs;
if (!self) {
QThread::currentThread()->quit();
return;
}
QMetaObject::invokeMethod(self.data(),
[self, runId, leftImg, rightImg, poseL, poseR, fusion, finalPose,
rightAttempted, rightSkippedByLeftRule, roiR, decision, totalMs,
captureLeftMs, captureRightMs, yoloLeftMs, grayLeftMs,
leftMatchWallMs, rightRoiPredictMs, grayRightMs,
rightMatchWallMs, fusionMs, startMs]() mutable {
if (!self || self->m_measureRunId != runId) return;
if (self->m_measureTimedOut) {
emit self->logMessage(QString::fromUtf8(
"[快速测量] 后台在5秒截止后结束,结果已丢弃。"));
return;
}
if (totalMs > kFastMeasureDeadlineMs) {
self->m_measureBusy = false;
self->ui.progressAlgo->setValue(0);
self->updateActionButtons();
emit self->logMessage(QString::fromUtf8(
"[快速测量][ERROR] 总耗时=%1ms,超过5秒,返回失败。").arg(totalMs));
emit self->measureCompleted(false);
return;
}
if (finalPose.valid) {
bool drawRight = rightAttempted && poseR.valid &&
(!poseL.valid || (!fusion.consistent && fusion.weightRight > fusion.weightLeft));
QImage poseFrame = drawRight ? rightImg : leftImg;
VisionPoseResult poseForDraw = drawRight ? poseR : finalPose;
if (poseFrame.isNull()) poseFrame = leftImg;
QImage poseImage;
cv::Mat poseVis = VisionGuideMatcher::qImageToMat(poseFrame);
if (!poseVis.empty()) {
self->m_visionMatcher->drawPoseResult(poseVis, poseForDraw);
poseImage = VisionGuideMatcher::matToQImage(poseVis);
}
// 将最终位姿图生成也纳入5秒全流程截止判断;日志和CSV在判定后写入。
const qint64 completionMs = QDateTime::currentMSecsSinceEpoch() - startMs;
if (completionMs > kFastMeasureDeadlineMs || poseImage.isNull()) {
self->m_measureBusy = false;
self->ui.progressAlgo->setValue(0);
self->updateActionButtons();
emit self->logMessage(QString::fromUtf8(
"[快速测量][ERROR] 位姿图生成后总耗时=%1ms,超过5秒或图像生成失败,返回失败。")
.arg(completionMs));
emit self->measureCompleted(false);
return;
}
emit self->logMessage(QString::fromUtf8(
"[快速测量][耗时] 左采图=%1ms 右采图=%2ms 左YOLO=%3ms 左灰度=%4ms "
"左匹配墙钟=%5ms/核心=%6ms 右ROI投影=%7ms 右灰度=%8ms "
"右匹配墙钟=%9ms/核心=%10ms 融合=%11ms 结果图后总计=%12ms")
.arg(captureLeftMs).arg(captureRightMs).arg(yoloLeftMs).arg(grayLeftMs)
.arg(leftMatchWallMs).arg(poseL.matchTimeMs, 0, 'f', 0)
.arg(rightRoiPredictMs).arg(grayRightMs)
.arg(rightMatchWallMs).arg(poseR.matchTimeMs, 0, 'f', 0)
.arg(fusionMs).arg(completionMs));
emit self->logMessage(QString::fromUtf8("[快速测量][决策] %1;右路执行=%2;右ROI=%3")
.arg(decision)
.arg(rightAttempted ? QString::fromUtf8("是") : QString::fromUtf8("否"))
.arg(roiR.isValid()
? QStringLiteral("(%1,%2,%3,%4)").arg(roiR.x()).arg(roiR.y()).arg(roiR.width()).arg(roiR.height())
: QStringLiteral("--")));
emit self->logMessage(poseLogLine(QStringLiteral("FAST_FINAL_OUTPUT"), finalPose));
self->m_lastFusion = fusion;
self->m_lastPose = finalPose;
self->displayPoseResult(finalPose);
self->storeResultImage(S_CAPTURE, poseFrame);
self->storeResultImage(S_POSE, poseImage);
self->showExternalResultImage(S_POSE, poseImage);
self->ui.tabWidget->setCurrentWidget(self->ui.tabVisionAlgo);
self->ui.editResultTime->setText(QString::number(completionMs));
self->m_coord->setCameraPose(finalPose);
self->appendFusionCsv(poseL, poseR, fusion, finalPose, completionMs);
self->ui.progressAlgo->setValue(100);
self->m_resultImagesReadyForExport = true;
emit self->logMessage(QString::fromUtf8(
"=== 快速测量完成 Score=%1 Total=%2ms ===")
.arg(finalPose.score, 0, 'f', 4).arg(completionMs));
} else {
self->ui.progressAlgo->setValue(0);
emit self->logMessage(QString::fromUtf8("[快速测量][ERROR] 位姿无效:%1")
.arg(finalPose.errorMsg));
}
self->m_measureBusy = false;
self->updateActionButtons();
emit self->measureCompleted(finalPose.valid);
}, Qt::QueuedConnection);
QThread::currentThread()->quit();
}, Qt::QueuedConnection);
m_measureThread = thread;
thread->start();
const qint64 elapsedBeforeWorkerMs = QDateTime::currentMSecsSinceEpoch() - startMs;
const int remainingMs = static_cast<int>(
std::max<qint64>(0, kFastMeasureDeadlineMs - elapsedBeforeWorkerMs));
QTimer::singleShot(remainingMs, this, [this, runId, startMs]() {
if (m_measureRunId != runId || !m_measureBusy || m_measureTimedOut) return;
m_measureTimedOut = true;
ui.progressAlgo->setValue(0);
ui.editResultTime->setText(QString::number(
QDateTime::currentMSecsSinceEpoch() - startMs));
emit logMessage(QString::fromUtf8(
"[快速测量][ERROR] 全流程达到5秒截止时间,按规则返回失败。"));
emit measureCompleted(false);
// 视觉引导同步调用不能安全强杀;后台退出前保持按钮禁用,防止并发访问matcher。
updateActionButtons();
});
}
// ── 结果图管理 ───────────────────────────────────────────────
void MeasurePanel::storeResultImage(const QString& name, const QImage& img, bool saveToExport) {
if (img.isNull())
return;
const bool replacingCurrent = (ui.comboResultView->currentText() == name);
m_resultImages[name] = img;
if (ui.comboResultView->findText(name) < 0)
ui.comboResultView->addItem(name);
if (saveToExport && ensureExportDir()) {
QString fn = name;
fn.replace(QRegExp("[^a-zA-Z0-9\\x4e00-\\x9fa5]"), "_");
const QString ts = QDateTime::currentDateTime().toString(QStringLiteral("yyyyMMdd_HHmmss_zzz"));
const QString fp = m_exportDir + "/" + fn + "_" + ts + ".png";
if (img.save(fp))
emit logMessage(QStringLiteral("[Measure] result image saved: %1").arg(fp));
else
emit logMessage(QStringLiteral("[Measure] result image save failed: %1").arg(fp));
}
if (replacingCurrent)
resetResultViewTransform();
}
void MeasurePanel::showExternalResultImage(const QString& name, const QImage& img)
{
if (img.isNull())
return;
storeResultImage(name, img);
resetResultViewTransform();
ui.comboResultView->setCurrentText(name);
showCurrentResultImage();
}
void MeasurePanel::showCurrentResultImage() {
QString name = ui.comboResultView->currentText();
if (m_resultImages.contains(name)) {
renderCurrentResultImage();
} else {
m_resultPanning = false;
ui.labelROIResult->clear();
ui.labelROIResult->setText("ROI / Result");
}
}
void MeasurePanel::onResultViewChanged(int)
{
resetResultViewTransform();
showCurrentResultImage();
}
void MeasurePanel::resetResultViewTransform()
{
m_resultZoom = 1.0;
m_resultPan = QPointF(0.0, 0.0);
m_resultPanning = false;
}
QRectF MeasurePanel::currentResultDrawRect() const
{
const QString name = ui.comboResultView->currentText();
const QImage img = m_resultImages.value(name);
const QSize view = ui.labelROIResult->size();
if (img.isNull() || img.width() <= 0 || img.height() <= 0 ||
view.width() <= 0 || view.height() <= 0)
return QRectF();
const double fit = (std::min)(double(view.width()) / double(img.width()),
double(view.height()) / double(img.height()));
const double scale = fit * m_resultZoom;
const QSizeF drawSize(double(img.width()) * scale, double(img.height()) * scale);
const QPointF topLeft((double(view.width()) - drawSize.width()) * 0.5 + m_resultPan.x(),
(double(view.height()) - drawSize.height()) * 0.5 + m_resultPan.y());
return QRectF(topLeft, drawSize);
}
void MeasurePanel::clampResultPan()
{
const QString name = ui.comboResultView->currentText();
const QImage img = m_resultImages.value(name);
const QSize view = ui.labelROIResult->size();
if (img.isNull() || img.width() <= 0 || img.height() <= 0 ||
view.width() <= 0 || view.height() <= 0) {
m_resultPan = QPointF(0.0, 0.0);
return;
}
if (m_resultZoom <= 1.0) {
m_resultZoom = 1.0;
m_resultPan = QPointF(0.0, 0.0);
return;
}
const double fit = (std::min)(double(view.width()) / double(img.width()),
double(view.height()) / double(img.height()));
const double drawW = double(img.width()) * fit * m_resultZoom;
const double drawH = double(img.height()) * fit * m_resultZoom;
const double maxX = (std::max)(0.0, (drawW - double(view.width())) * 0.5);
const double maxY = (std::max)(0.0, (drawH - double(view.height())) * 0.5);
m_resultPan.setX(qBound(-maxX, m_resultPan.x(), maxX));
m_resultPan.setY(qBound(-maxY, m_resultPan.y(), maxY));
}
void MeasurePanel::renderCurrentResultImage()
{
const QString name = ui.comboResultView->currentText();
const QImage img = m_resultImages.value(name);
const QSize view = ui.labelROIResult->size();
if (img.isNull() || view.width() <= 0 || view.height() <= 0) {
ui.labelROIResult->clear();
ui.labelROIResult->setText("ROI / Result");
return;
}
clampResultPan();
QPixmap canvas(view);
canvas.fill(QColor(28, 31, 36));
QPainter painter(&canvas);
painter.setRenderHint(QPainter::SmoothPixmapTransform, true);
painter.drawImage(currentResultDrawRect(), img);
painter.end();
ui.labelROIResult->setPixmap(canvas);
}
bool MeasurePanel::eventFilter(QObject* obj, QEvent* event)
{
if (obj != ui.labelROIResult)
return QObject::eventFilter(obj, event);
const QString name = ui.comboResultView->currentText();
if (!m_resultImages.contains(name) || m_resultImages.value(name).isNull())
return QObject::eventFilter(obj, event);
if (event->type() == QEvent::Wheel) {
auto* we = static_cast<QWheelEvent*>(event);
const QPointF cursor = we->pos();
const QRectF before = currentResultDrawRect();
if (!before.isValid())
return true;
const double nx = (cursor.x() - before.left()) / before.width();
const double ny = (cursor.y() - before.top()) / before.height();
const int delta = we->angleDelta().y() != 0 ? we->angleDelta().y() : we->angleDelta().x();
if (delta == 0)
return true;
const double factor = std::pow(1.15, double(delta) / 120.0);
m_resultZoom = qBound(1.0, m_resultZoom * factor, 20.0);
const QSize view = ui.labelROIResult->size();
const QImage img = m_resultImages.value(name);
const double fit = (std::min)(double(view.width()) / double(img.width()),
double(view.height()) / double(img.height()));
const QSizeF drawSize(double(img.width()) * fit * m_resultZoom,
double(img.height()) * fit * m_resultZoom);
m_resultPan = QPointF(cursor.x() - nx * drawSize.width() - (double(view.width()) - drawSize.width()) * 0.5,
cursor.y() - ny * drawSize.height() - (double(view.height()) - drawSize.height()) * 0.5);
renderCurrentResultImage();
event->accept();
return true;
}
if (event->type() == QEvent::MouseButtonDblClick) {
resetResultViewTransform();
renderCurrentResultImage();
event->accept();
return true;
}
if (event->type() == QEvent::MouseButtonPress) {
auto* me = static_cast<QMouseEvent*>(event);
if (me->button() == Qt::LeftButton && m_resultZoom > 1.0) {
m_resultPanning = true;
m_resultPanStart = me->pos();
m_resultPanStartOffset = m_resultPan;
ui.labelROIResult->setCursor(Qt::ClosedHandCursor);
event->accept();
return true;
}
}
if (event->type() == QEvent::MouseMove && m_resultPanning) {
auto* me = static_cast<QMouseEvent*>(event);
m_resultPan = m_resultPanStartOffset + QPointF(me->pos() - m_resultPanStart);
renderCurrentResultImage();
event->accept();
return true;
}
if (event->type() == QEvent::MouseButtonRelease && m_resultPanning) {
auto* me = static_cast<QMouseEvent*>(event);
if (me->button() == Qt::LeftButton) {
m_resultPanning = false;
ui.labelROIResult->unsetCursor();
event->accept();
return true;
}
}
if (event->type() == QEvent::Resize) {
renderCurrentResultImage();
return QObject::eventFilter(obj, event);
}
return QObject::eventFilter(obj, event);
}
void MeasurePanel::rememberPoseOverlaySource(const QImage& image,
const VisionPoseResult& pose,
CameraSide side)
{
if (image.isNull() || !pose.valid)
return;
m_poseOverlayBaseImage = image;
m_poseOverlayCenterPose = pose;
m_poseOverlaySide = side;
ui.groupResult->setTitle(QString::fromUtf8(
"位姿估计结果 - 零件中心(相机坐标系)"));
}
bool MeasurePanel::showGraspPointOverlay(const VisionPoseResult& pose)
{
if (!pose.valid || m_poseOverlayBaseImage.isNull() ||
!m_poseOverlayCenterPose.valid || !m_visionMatcher)
return false;
VisionPoseResult overlayPose;
if (!m_coord->makeGraspPose(m_poseOverlayCenterPose, overlayPose))
return false;
// 保持实物轮廓与原图对齐,只把坐标原点和三轴移动到实际抓取点。
// updateContours=false 会保留零件中心位姿生成的原始投影轮廓。
if (!m_visionMatcher->updatePoseProjection(overlayPose, m_poseOverlaySide, false))
return false;
cv::Mat vis = VisionGuideMatcher::qImageToMat(m_poseOverlayBaseImage);
if (vis.empty())
return false;
m_visionMatcher->drawPoseResult(vis, overlayPose);
cv::putText(vis,
cv::format("GRASP: local +Z %.3f mm", ui.spinGraspOffsetZMm->value()),
cv::Point(15, 104), cv::FONT_HERSHEY_SIMPLEX, 0.60,
cv::Scalar(0, 200, 255), 2, cv::LINE_AA);
const QImage image = VisionGuideMatcher::matToQImage(vis);
if (image.isNull())
return false;
storeResultImage(S_GRASP_POSE, image);
ui.comboResultView->setCurrentText(S_GRASP_POSE);
showCurrentResultImage();
m_resultImagesReadyForExport = true;
updateActionButtons();
return true;
}
void MeasurePanel::onPoseDisplayRequested(const VisionPoseResult& pose,
const QString& mode)
{
if (!pose.valid)
return;
m_displayPose = pose;
displayPoseResult(pose, mode == QStringLiteral("robot"));
if (mode == QStringLiteral("center")) {
ui.groupResult->setTitle(QString::fromUtf8(
"位姿估计结果 - 零件中心(相机坐标系)"));
if (m_resultImages.contains(S_POSE)) {
ui.comboResultView->setCurrentText(S_POSE);
showCurrentResultImage();
}
} else if (mode == QStringLiteral("grasp")) {
ui.groupResult->setTitle(QString::fromUtf8(
"位姿估计结果 - 实际抓取点(相机坐标系)"));
if (!showGraspPointOverlay(pose))
emit logMessage(QString::fromUtf8(
"[抓取点显示警告] XYZ 已转换,但当前缺少可重新投影的原图或相机参数,未更新结果图。"));
} else if (mode == QStringLiteral("robot")) {
const QString frameLabel = m_coord->outputReferenceFrame() == QStringLiteral("user")
? QString::fromUtf8("用户坐标系") : QString::fromUtf8("世界坐标系");
ui.groupResult->setTitle(QString::fromUtf8(
"抓取位姿 XYZABC - %1").arg(frameLabel));
}
}
void MeasurePanel::displayPoseResult(const VisionPoseResult& p, bool estunAbc) {
if (!p.valid) {
ui.editResultX->clear();
ui.editResultY->clear();
ui.editResultZ->clear();
ui.editResultAxis->clear();
return;
}
ui.editResultX->setText(QString::number(p.tx*1000,'f',3));
ui.editResultY->setText(QString::number(p.ty*1000,'f',3));
ui.editResultZ->setText(QString::number(p.tz*1000,'f',3));
ui.editResultAxis->setText(estunAbc
? QString("A=%1 B=%2 C=%3")
.arg(p.rx,0,'f',3).arg(p.ry,0,'f',3).arg(p.rz,0,'f',3)
: QString("Rx=%1 Ry=%2 Rz=%3")
.arg(p.rx,0,'f',2).arg(p.ry,0,'f',2).arg(p.rz,0,'f',2));
}
// ── 导出 ─────────────────────────────────────────────────────
static QString poseCsvFields(const VisionPoseResult& p)
{
return QStringLiteral("%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12")
.arg(p.valid ? 1 : 0)
.arg(p.tx * 1000.0, 0, 'f', 6)
.arg(p.ty * 1000.0, 0, 'f', 6)
.arg(p.tz * 1000.0, 0, 'f', 6)
.arg(p.rx, 0, 'f', 6)
.arg(p.ry, 0, 'f', 6)
.arg(p.rz, 0, 'f', 6)
.arg(p.score, 0, 'f', 6)
.arg(p.stdTx, 0, 'f', 6)
.arg(p.stdTy, 0, 'f', 6)
.arg(p.stdTz, 0, 'f', 6)
.arg(p.matchTimeMs, 0, 'f', 0);
}
void MeasurePanel::appendFusionCsv(const VisionPoseResult& leftPose,
const VisionPoseResult& rightPose,
const StereoPoseFusion::FusionResult& fusion,
const VisionPoseResult& finalPose,
qint64 totalMs)
{
if (!ensureExportDir()) {
emit logMessage(QStringLiteral("[Measure] Fusion CSV write skipped: export directory unavailable."));
return;
}
auto headerFor = [](const QString& prefix) {
return QStringLiteral("%1_valid,%1_x_mm,%1_y_mm,%1_z_mm,%1_rx_deg,%1_ry_deg,%1_rz_deg,%1_score,%1_std_tx_mm,%1_std_ty_mm,%1_std_tz_mm,%1_match_ms")
.arg(prefix);
};
const QString header =
QStringLiteral("timestamp,total_ms,") +
headerFor(QStringLiteral("left_raw")) + QStringLiteral(",") +
headerFor(QStringLiteral("right_raw")) + QStringLiteral(",") +
headerFor(QStringLiteral("right_in_left")) + QStringLiteral(",") +
headerFor(QStringLiteral("fused")) + QStringLiteral(",") +
QStringLiteral("dt_mm,daxis_deg,consistent,w_left,w_right,confidence,final_valid\n");
const QString row =
QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss.zzz") + QStringLiteral(",") +
QString::number(totalMs) + QStringLiteral(",") +
poseCsvFields(leftPose) + QStringLiteral(",") +
poseCsvFields(rightPose) + QStringLiteral(",") +
poseCsvFields(fusion.rightInLeft) + QStringLiteral(",") +
poseCsvFields(finalPose) + QStringLiteral(",") +
QString::number(fusion.positionDisparityMm, 'f', 6) + QStringLiteral(",") +
QString::number(fusion.orientationDisparityDeg, 'f', 6) + QStringLiteral(",") +
QString::number(fusion.consistent ? 1 : 0) + QStringLiteral(",") +
QString::number(fusion.weightLeft, 'f', 6) + QStringLiteral(",") +
QString::number(fusion.weightRight, 'f', 6) + QStringLiteral(",") +
QString::number(fusion.confidence, 'f', 6) + QStringLiteral(",") +
QString::number(finalPose.valid ? 1 : 0) + QStringLiteral("\n");
auto tryWriteCsv = [&](const QString& path, QString* errorText) -> bool {
const bool writeHeader = !QFileInfo::exists(path) || QFileInfo(path).size() == 0;
QFile f(path);
if (!f.open(QIODevice::WriteOnly | QIODevice::Append | QIODevice::Text)) {
if (errorText)
*errorText = f.errorString();
return false;
}
QTextStream out(&f);
if (writeHeader)
out << header;
out << row;
out.flush();
const bool flushed = f.flush();
if (!flushed && errorText)
*errorText = f.errorString();
f.close();
return flushed;
};
const QString fp = m_exportDir + "/fusion_measure_log.csv";
QString errorText;
if (tryWriteCsv(fp, &errorText)) {
emit logMessage(QStringLiteral("[Measure] Fusion CSV appended: %1").arg(fp));
return;
}
emit logMessage(QStringLiteral("[Measure] Fusion CSV open/write failed: %1 (%2)")
.arg(fp, errorText));
const QString backup = m_exportDir + "/fusion_measure_log_" +
QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss_zzz") + ".csv";
errorText.clear();
if (tryWriteCsv(backup, &errorText)) {
emit logMessage(QStringLiteral("[Measure] Fusion CSV appended to fallback file: %1").arg(backup));
} else {
emit logMessage(QStringLiteral("[Measure] Fusion CSV fallback failed: %1 (%2)")
.arg(backup, errorText));
}
}
bool MeasurePanel::ensureExportDir() {
if (m_exportDir.isEmpty())
m_exportDir = QCoreApplication::applicationDirPath() + "/export";
QDir d(m_exportDir);
return d.exists() || d.mkpath(".");
}
void MeasurePanel::onBrowseExportDir() {
QString d = AppSettings::instance().openDir(
qobject_cast<QWidget*>(parent()), AppSettings::K_ExportDir,
QString::fromUtf8("选择导出目录"), m_exportDir);
if (!d.isEmpty()) { m_exportDir = d; ui.editExportDir->setText(d); }
}
void MeasurePanel::onExportPoseCamera() {
if (!ensureExportDir()) return;
VisionPoseResult pose = m_coord ? m_coord->activeCameraPose() : m_lastPose;
if (!pose.valid) pose = m_lastPose;
if (!pose.valid) return;
const bool graspPoint = m_coord && m_coord->activeCameraPoseIsGraspPoint();
QString ts = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss");
QString fp = m_exportDir + (graspPoint ? "/pose_camera_grasp_" : "/pose_camera_center_") + ts + ".txt";
QFile f(fp);
if (!f.open(QIODevice::WriteOnly | QIODevice::Text)) return;
QTextStream out(&f);
out << "PointType: " << (graspPoint ? "grasp" : "part_center") << "\n"
<< "CoordinateFrame: camera\n"
<< "X: " << QString::number(pose.tx*1000,'f',3) << " mm\n"
<< "Y: " << QString::number(pose.ty*1000,'f',3) << " mm\n"
<< "Z: " << QString::number(pose.tz*1000,'f',3) << " mm\n"
<< "Rx: " << QString::number(pose.rx,'f',4) << " deg\n"
<< "Ry: " << QString::number(pose.ry,'f',4) << " deg\n"
<< "Rz: " << QString::number(pose.rz,'f',4) << " deg\n"
<< "Score: " << QString::number(pose.score,'f',6) << "\n";
f.close();
emit logMessage(QString::fromUtf8("位姿已导出: ") + fp);
}
void MeasurePanel::onExportPoseRobot() {
if (!ensureExportDir() || !m_coord->robotPoseReady()) return;
auto rp = m_coord->robotPose();
const QString referenceFrame = m_coord->outputReferenceFrame();
const QString frameLabel = referenceFrame == QStringLiteral("user")
? QString::fromUtf8("用户坐标系") : QString::fromUtf8("世界坐标系");
QString ts = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss");
QString fp = m_exportDir + "/pose_grasp_" + referenceFrame + "_" + ts + ".csv";
QFile f(fp);
if (!f.open(QIODevice::WriteOnly | QIODevice::Text)) return;
QTextStream out(&f);
out << "时间,参考坐标系,X_mm,Y_mm,Z_mm,A_deg,B_deg,C_deg\n"
<< QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss") << ","
<< frameLabel << ","
<< QString::number(rp.tx*1000,'f',3) << "," << QString::number(rp.ty*1000,'f',3) << ","
<< QString::number(rp.tz*1000,'f',3) << "," << QString::number(rp.rx,'f',4) << ","
<< QString::number(rp.ry,'f',4) << "," << QString::number(rp.rz,'f',4) << "\n";
f.close();
emit logMessage(QString::fromUtf8("抓取位姿 XYZABC 已导出: ") + fp);
}
void MeasurePanel::onExportResultImages()
{
if (!m_resultImagesReadyForExport || m_resultImages.isEmpty()) {
emit logMessage(QString::fromUtf8("[Measure] 暂无可导出的全流程结果图。"));
return;
}
if (!ensureExportDir())
return;
const QString ts = QDateTime::currentDateTime().toString(QStringLiteral("yyyyMMdd_HHmmss"));
int saved = 0;
for (auto it = m_resultImages.constBegin(); it != m_resultImages.constEnd(); ++it) {
if (it.value().isNull())
continue;
QString fn = it.key();
fn.replace(QRegExp("[^a-zA-Z0-9\\x4e00-\\x9fa5]"), "_");
const QString fp = m_exportDir + "/" + fn + "_" + ts + ".jpg";
if (it.value().save(fp, "JPG", 92))
++saved;
else
emit logMessage(QString::fromUtf8("[Measure] 结果图导出失败: ") + fp);
}
emit logMessage(QString::fromUtf8("[Measure] 结果图已导出 JPG: %1 张 -> %2")
.arg(saved)
.arg(m_exportDir));
}
void MeasurePanel::onROIContextMenu(const QPoint& pos) {
QString cur = ui.comboResultView->currentText();
if (!m_resultImages.contains(cur) || m_resultImages[cur].isNull()) return;
QMenu menu;
QAction* act = menu.addAction(QString::fromUtf8("导出「%1」为 PNG").arg(cur));
connect(act, &QAction::triggered, [this, cur]{
if (!ensureExportDir()) return;
QString ts = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss");
QString fn = cur; fn.replace(QRegExp("[^a-zA-Z0-9\\x4e00-\\x9fa5]"), "_");
m_resultImages[cur].save(m_exportDir + "/" + fn + "_" + ts + ".png");
emit logMessage(QString::fromUtf8("图片已导出"));
});
menu.exec(ui.labelROIResult->mapToGlobal(pos));
}