#include "GroovePanel.h" #include "ui_MainWindow.h" #include "CameraManager.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace { QString stateText(int id) { return id == 0 ? QString::fromUtf8("空凹槽") : QString::fromUtf8("已占用"); } QColor stateColor(int id) { return id == 0 ? QColor(22,119,255) : QColor(245,34,45); } qreal rectIou(const QRectF& a, const QRectF& b) { const QRectF intersection = a.intersected(b); const qreal inter = qMax(0.0, intersection.width()) * qMax(0.0, intersection.height()); const qreal uni = a.width() * a.height() + b.width() * b.height() - inter; return uni > 0.0 ? inter / uni : 0.0; } bool axisEndpoints2d(const QVector& points, QPointF& endpointA, QPointF& endpointB) { if (points.size() != 4) return false; static const int pairings[3][4] = { {0, 1, 2, 3}, {0, 2, 1, 3}, {0, 3, 1, 2} }; int best = -1; double bestScore = std::numeric_limits::infinity(); for (int i = 0; i < 3; ++i) { const auto& q = pairings[i]; const double d1 = std::hypot(points[q[0]].x() - points[q[1]].x(), points[q[0]].y() - points[q[1]].y()); const double d2 = std::hypot(points[q[2]].x() - points[q[3]].x(), points[q[2]].y() - points[q[3]].y()); if (d1 + d2 < bestScore) { bestScore = d1 + d2; best = i; } } if (best < 0) return false; const auto& q = pairings[best]; endpointA = (points[q[0]] + points[q[1]]) / 2.0; endpointB = (points[q[2]] + points[q[3]]) / 2.0; return std::hypot(endpointA.x() - endpointB.x(), endpointA.y() - endpointB.y()) > 2.0; } } GroovePanel::GroovePanel(Ui::MainWindowClass& u, CameraManager* cameraManager, VisionGuideMatcher* matcher, QObject* parent) : QObject(parent), ui(u), m_cameraManager(cameraManager), m_matcher(matcher), m_occupancy(std::make_unique()), m_pose(std::make_unique()) { buildTab(); connect(ui.btnLoadGrooveModel, &QPushButton::clicked, this, &GroovePanel::onLoadOccupancyModel); connect(ui.btnRunGrooveOccupancy, &QPushButton::clicked, this, &GroovePanel::onRunOccupancy); if (m_cameraManager) { connect(m_cameraManager, &CameraManager::cameraConnected, this, [this](CameraManager::CamIndex){ updateState(); }); connect(m_cameraManager, &CameraManager::cameraDisconnected, this, [this](CameraManager::CamIndex){ updateState(); }); } QTimer::singleShot(0, this, &GroovePanel::autoLoadModels); updateState(); } void GroovePanel::buildTab() { QWidget* host = qobject_cast(parent()); m_tab = new QWidget(host); auto* layout = new QVBoxLayout(m_tab); QWidget* oldParent = ui.groupGrooveOccupancy->parentWidget(); if (oldParent && oldParent->layout()) oldParent->layout()->removeWidget(ui.groupGrooveOccupancy); ui.groupGrooveOccupancy->setParent(m_tab); layout->addWidget(ui.groupGrooveOccupancy); ui.tableGrooveResults->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); ui.tableGrooveResults->verticalHeader()->setVisible(false); ui.tableGrooveResults->setSelectionBehavior(QAbstractItemView::SelectRows); ui.tableGrooveResults->setEditTriggers(QAbstractItemView::NoEditTriggers); auto* poseGroup = new QGroupBox(QString::fromUtf8("凹槽定位(YOLO框中心双目定位 + 方向辅助)"), m_tab); auto* poseLayout = new QVBoxLayout(poseGroup); auto* modelRow = new QHBoxLayout; m_poseModelPath = new QLabel(QString::fromUtf8("方向辅助模型:正在检查精度版 1280 ONNX"), poseGroup); auto* loadPose = new QPushButton(QString::fromUtf8("加载/更换位姿模型"), poseGroup); modelRow->addWidget(m_poseModelPath, 1); modelRow->addWidget(loadPose); poseLayout->addLayout(modelRow); m_poseModelStatus = new QLabel(QString::fromUtf8("● 方向模型未加载(仍可输出中心三维位置)"), poseGroup); m_poseModelStatus->setStyleSheet(QStringLiteral("color: gray;")); poseLayout->addWidget(m_poseModelStatus); auto* actionRow = new QHBoxLayout; m_runPose = new QPushButton(QString::fromUtf8("执行空凹槽定位"), poseGroup); m_runPose->setStyleSheet(QStringLiteral("QPushButton {background-color:#13a8a8;color:white;font-weight:bold;}")); m_poseStatus = new QLabel(QString::fromUtf8("定位结果:未执行"), poseGroup); actionRow->addWidget(m_runPose); actionRow->addWidget(m_poseStatus, 1); poseLayout->addLayout(actionRow); m_poseTable = new QTableWidget(poseGroup); m_poseTable->setColumnCount(8); m_poseTable->setHorizontalHeaderLabels({QString::fromUtf8("凹槽"), QString::fromUtf8("定位依据"), QString::fromUtf8("中心 X/Y/Z (mm)"), QString::fromUtf8("中轴 Z"), QString::fromUtf8("平面法向 Y"), QString::fromUtf8("X 轴"), QString::fromUtf8("中心光线误差(mm)"), QString::fromUtf8("状态")}); m_poseTable->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); m_poseTable->verticalHeader()->setVisible(false); m_poseTable->setEditTriggers(QAbstractItemView::NoEditTriggers); poseLayout->addWidget(m_poseTable); layout->addWidget(poseGroup, 1); layout->addStretch(); connect(loadPose, &QPushButton::clicked, this, &GroovePanel::onLoadPoseModel); connect(m_runPose, &QPushButton::clicked, this, &GroovePanel::onRunPoseLocalization); const int yoloIndex = ui.tabWidget->indexOf(ui.tabYoloDetect); ui.tabWidget->insertTab(yoloIndex < 0 ? ui.tabWidget->count() : yoloIndex + 1, m_tab, QString::fromUtf8("凹槽占位")); } QString GroovePanel::defaultOccupancyModelPath() { const QDir cwd(QDir::currentPath()); const QDir app(QCoreApplication::applicationDirPath()); const QString rel = QStringLiteral("Yolo/groove_best.onnx"); for (const QString& p : {cwd.absoluteFilePath(rel), app.absoluteFilePath(rel), app.absoluteFilePath(QStringLiteral("../../") + rel)}) if (QFileInfo::exists(p)) return QDir::cleanPath(p); return {}; } QString GroovePanel::defaultPoseModelPath() { const QDir cwd(QDir::currentPath()); const QDir app(QCoreApplication::applicationDirPath()); const QStringList relativePaths = { QStringLiteral("Groove_pose/05_export/groove_pose_precision_1280.onnx"), QStringLiteral("Groove_pose/13_precision_refinement_20260715/runs/combined_precision_yolov8n_1280/weights/best.onnx"), QStringLiteral("Groove_pose/05_export/groove_pose_best.onnx") }; for (const QString& rel : relativePaths) { for (const QString& p : {cwd.absoluteFilePath(rel), app.absoluteFilePath(rel), app.absoluteFilePath(QStringLiteral("../../") + rel)}) if (QFileInfo::exists(p)) return QDir::cleanPath(p); } return {}; } QString GroovePanel::defaultCalibPath(const QString& fileName) { const QDir cwd(QDir::currentPath()); const QDir app(QCoreApplication::applicationDirPath()); const QString rel = QStringLiteral("StereoCalib/") + fileName; for (const QString& p : {cwd.absoluteFilePath(rel), app.absoluteFilePath(rel), app.absoluteFilePath(QStringLiteral("../../") + rel)}) if (QFileInfo::exists(p)) return QDir::cleanPath(p); return {}; } void GroovePanel::autoLoadModels() { const QString occupancy = defaultOccupancyModelPath(); if (!occupancy.isEmpty()) loadOccupancyModel(occupancy); else emit logMessage(QString::fromUtf8("[凹槽占位] 未找到 Yolo/groove_best.onnx。")); const QString pose = defaultPoseModelPath(); if (!pose.isEmpty()) loadPoseModel(pose); else emit logMessage(QString::fromUtf8("[凹槽定位] 未找到方向辅助模型;仍可使用左右占位框中心输出三维位置。")); } bool GroovePanel::loadOccupancyModel(const QString& path) { if (!m_occupancy->loadModel(path)) { m_occupancyLoaded = false; emit logMessage(QString::fromUtf8("[凹槽占位错误] ") + m_occupancy->lastError()); updateState(); return false; } m_occupancyLoaded = true; ui.labelGrooveModelPath->setText(path); emit logMessage(QString::fromUtf8("[凹槽占位] 独立模型加载成功:%1;类别=empty_groove、occupied_groove。") .arg(QFileInfo(path).fileName())); updateState(); return true; } bool GroovePanel::loadPoseModel(const QString& path) { if (!m_pose->loadModel(path)) { m_poseLoaded = false; emit logMessage(QString::fromUtf8("[凹槽定位错误] ") + m_pose->lastError()); updateState(); return false; } m_poseLoaded = true; m_poseModelPath->setText(path); emit logMessage(QString::fromUtf8("[凹槽定位] 四关键点模型加载成功:%1;输入=%2×%3。") .arg(QFileInfo(path).fileName()).arg(m_pose->inputWidth()).arg(m_pose->inputHeight())); updateState(); return true; } void GroovePanel::onLoadOccupancyModel() { const QString p = QFileDialog::getOpenFileName(qobject_cast(parent()), QString::fromUtf8("选择凹槽占位识别 ONNX 模型"), defaultOccupancyModelPath(), QString::fromUtf8("ONNX 模型 (*.onnx);;All Files (*)")); if (!p.isEmpty()) loadOccupancyModel(p); } void GroovePanel::onLoadPoseModel() { const QString p = QFileDialog::getOpenFileName(qobject_cast(parent()), QString::fromUtf8("选择凹槽四关键点 ONNX 模型"), defaultPoseModelPath(), QString::fromUtf8("ONNX 模型 (*.onnx);;All Files (*)")); if (!p.isEmpty()) loadPoseModel(p); } void GroovePanel::updateState() { const bool left = m_cameraManager && m_cameraManager->isConnected(CameraManager::LEFT); const bool right = m_cameraManager && m_cameraManager->isConnected(CameraManager::RIGHT); ui.btnRunGrooveOccupancy->setEnabled(m_occupancyLoaded && left); m_runPose->setEnabled(m_occupancyLoaded && left && right); ui.labelGrooveModelStatus->setText(m_occupancyLoaded ? QString::fromUtf8("● 凹槽模型已加载(空槽 / 已占用)") : QString::fromUtf8("● 凹槽模型未加载")); ui.labelGrooveModelStatus->setStyleSheet(m_occupancyLoaded ? QStringLiteral("color:#00aa55;") : QStringLiteral("color:gray;")); m_poseModelStatus->setText(m_poseLoaded ? QString::fromUtf8("● 方向辅助模型已加载(4关键点,仅用于中轴方向)") : QString::fromUtf8("● 方向模型未加载(仍可输出中心三维位置)")); m_poseModelStatus->setStyleSheet(m_poseLoaded ? QStringLiteral("color:#00aa55;") : QStringLiteral("color:gray;")); } QImage GroovePanel::captureLeft(QString* sourceName) const { if (m_cameraManager && m_cameraManager->isConnected(CameraManager::LEFT)) { if (sourceName) *sourceName = QString::fromUtf8("左相机实时单帧"); return m_cameraManager->captureSingleFrame(CameraManager::LEFT); } return {}; } QImage GroovePanel::captureRight(QString* sourceName) const { if (m_cameraManager && m_cameraManager->isConnected(CameraManager::RIGHT)) { if (sourceName) *sourceName = QString::fromUtf8("右相机实时单帧"); return m_cameraManager->captureSingleFrame(CameraManager::RIGHT); } return {}; } QVector GroovePanel::detectOccupancy(const QImage& image) const { if (!m_occupancyLoaded || image.isNull()) return {}; return m_occupancy->detect(image, 0.25f, 0.45f); } QRect GroovePanel::cropRectForBox(const QRectF& box, const QSize& size) { QRectF e = box; // 与训练数据保持一致:总宽高扩大 50%,即四周各扩大原框的 25%。 e.adjust(-box.width() * .25, -box.height() * .25, box.width() * .25, box.height() * .25); int x = qBound(0, static_cast(std::floor(e.left())), size.width() - 1); int y = qBound(0, static_cast(std::floor(e.top())), size.height() - 1); int r = qBound(x + 1, static_cast(std::ceil(e.right())), size.width()); int b = qBound(y + 1, static_cast(std::ceil(e.bottom())), size.height()); return QRect(x, y, r - x, b - y); } bool GroovePanel::inferFourPoints(const QImage& image, const QRectF& box, QVector& points, double* poseConfidence, double* poseIou) const { points.clear(); if (poseConfidence) *poseConfidence = 0.0; if (poseIou) *poseIou = 0.0; if (!m_poseLoaded || image.isNull()) return false; const QRect crop = cropRectForBox(box, image.size()); const auto predictions = m_pose->detect(image.copy(crop), 0.25f, 0.45f); if (predictions.isEmpty()) return false; const QRectF target = box.translated(-crop.x(), -crop.y()); const GroovePosePrediction* best = nullptr; qreal bestIou = -1.0; for (const auto& prediction : predictions) { const qreal iou = rectIou(prediction.box, target); if (!best || iou > bestIou || (qFuzzyCompare(iou, bestIou) && prediction.confidence > best->confidence)) { best = &prediction; bestIou = iou; } } if (!best) return false; if (poseConfidence) *poseConfidence = best->confidence; if (poseIou) *poseIou = bestIou; if (best->confidence < 0.85f || bestIou < 0.50) return false; for (const QPointF& p : best->keypoints) points.append(p + crop.topLeft()); // 保留模型 P0/P1/P2/P3 的原始物理语义,不再按图像位置循环重排。 int reliable = 0; for (float c : best->keypointConfidence) if (c >= 0.35f) ++reliable; return points.size() == 4 && reliable == 4; } QImage GroovePanel::drawOccupancy(const QImage& image, const QVector& detections) const { QImage out = image.copy(); QPainter p(&out); p.setRenderHint(QPainter::Antialiasing); const int width = qMax(3, out.width() / 900); for (const auto& d : detections) { const QColor c = stateColor(d.classId); p.setBrush(Qt::NoBrush); p.setPen(QPen(c, width)); p.drawRect(d.box); p.setBrush(c); p.drawEllipse(d.box.center(), width * 2.0, width * 2.0); p.setPen(Qt::white); p.drawText(d.box.topLeft() + QPointF(4, 20), stateText(d.classId)); } return out; } void GroovePanel::fillOccupancyTable(const QVector& detections) { int empty = 0, occupied = 0; ui.tableGrooveResults->setRowCount(detections.size()); for (int i = 0; i < detections.size(); ++i) { const auto& d = detections[i]; if (d.classId == 0) ++empty; else ++occupied; const QPointF c = d.box.center(); const QStringList values = {QString::number(i + 1), stateText(d.classId), QString::number(d.confidence, 'f', 3), QString::number(c.x(), 'f', 1), QString::number(c.y(), 'f', 1), QStringLiteral("x=%1 y=%2 w=%3 h=%4") .arg(d.box.x(),0,'f',1).arg(d.box.y(),0,'f',1).arg(d.box.width(),0,'f',1).arg(d.box.height(),0,'f',1)}; for (int col = 0; col < values.size(); ++col) { auto* item = new QTableWidgetItem(values[col]); item->setTextAlignment(Qt::AlignCenter); if (col == 1) item->setForeground(stateColor(d.classId)); ui.tableGrooveResults->setItem(i, col, item); } } ui.editEmptyGrooveCount->setText(QString::number(empty)); ui.editOccupiedGrooveCount->setText(QString::number(occupied)); ui.labelGrooveResultStatus->setText(QString::fromUtf8("识别结果:空凹槽 %1 个,已占用 %2 个,总计 %3 个") .arg(empty).arg(occupied).arg(detections.size())); } void GroovePanel::onRunOccupancy() { QString source; const QImage image = captureLeft(&source); if (image.isNull()) { emit logMessage(QString::fromUtf8("[凹槽占位] 左相机采集失败。")); return; } QElapsedTimer timer; timer.start(); m_lastOccupancy = detectOccupancy(image); const qint64 ms = timer.elapsed(); fillOccupancyTable(m_lastOccupancy); ui.editGrooveDetectTime->setText(QString::number(ms)); QImage annotated = drawOccupancy(image, m_lastOccupancy); emit resultImage(QString::fromUtf8("凹槽占位识别"), annotated); emit logMessage(QString::fromUtf8("[凹槽占位] 检测完成:总数=%1,耗时=%2ms,来源=%3。") .arg(m_lastOccupancy.size()).arg(ms).arg(source)); } bool GroovePanel::ensureCalibration() { if (!m_matcher) return false; if (!m_matcher->isCamLoaded(CAM_LEFT)) { const QString p = defaultCalibPath(QStringLiteral("cam_left.cal")); if (p.isEmpty() || !m_matcher->loadCameraParams(p, CAM_LEFT)) return false; } if (!m_matcher->isCamLoaded(CAM_RIGHT)) { const QString p = defaultCalibPath(QStringLiteral("cam_right.cal")); if (p.isEmpty() || !m_matcher->loadCameraParams(p, CAM_RIGHT)) return false; } return !defaultCalibPath(QStringLiteral("pos_right2left.dat")).isEmpty(); } double GroovePanel::norm(const cv::Point3d& p) { return std::sqrt(p.dot(p)); } cv::Point3d GroovePanel::normalize(const cv::Point3d& p) { const double n = norm(p); return n > 1e-9 ? p * (1.0 / n) : cv::Point3d(0,0,0); } QString GroovePanel::vecText(const cv::Point3d& p) { return QStringLiteral("(%1,%2,%3)").arg(p.x,0,'f',2).arg(p.y,0,'f',2).arg(p.z,0,'f',2); } void GroovePanel::fillPoseTable(const QVector& poseSlots) { m_poseTable->setRowCount(poseSlots.size()); for (int i = 0; i < poseSlots.size(); ++i) { const auto& s = poseSlots[i]; const QString basis = !s.positionValid ? QStringLiteral("--") : s.axisObserved ? QString::fromUtf8("框中心 + 本槽方向") : s.directionValid ? QString::fromUtf8("框中心 + 场景共识") : QString::fromUtf8("左右框中心"); const cv::Point3d xAxis = (s.directionValid && s.normalValid) ? normalize(s.normal.cross(s.axis)) : cv::Point3d(); const QStringList vals = { QStringLiteral("G%1").arg(i + 1), basis, s.positionValid ? vecText(s.center) : QStringLiteral("--"), s.directionValid ? vecText(s.axis) : QStringLiteral("--"), s.normalValid ? vecText(s.normal) : QStringLiteral("--"), s.normalValid ? vecText(xAxis) : QStringLiteral("--"), s.positionValid ? QString::number(s.centerRayErrorMm, 'f', 3) : QStringLiteral("--"), s.status }; for (int c = 0; c < vals.size(); ++c) { auto* item = new QTableWidgetItem(vals[c]); item->setTextAlignment(Qt::AlignCenter); if (c == 7) { item->setForeground(s.status.startsWith(QStringLiteral("VALID")) ? QColor(0,160,80) : s.positionValid ? QColor(20,120,220) : QColor(220,80,40)); } m_poseTable->setItem(i, c, item); } } } QImage GroovePanel::drawPose(const QImage& image, const QVector& poseSlots) const { QImage out = image.copy(); QPainter p(&out); p.setRenderHint(QPainter::Antialiasing); const int w = qMax(3, out.width() / 900); QFont font = p.font(); font.setPixelSize(qMax(20, out.width() / 180)); p.setFont(font); for (const auto& s : poseSlots) { const QColor slotColor = s.positionValid ? QColor(0,210,130) : QColor(240,90,70); p.setBrush(Qt::NoBrush); p.setPen(QPen(slotColor, w)); if (s.leftBox.isValid()) p.drawRect(s.leftBox); if (s.positionValid && s.leftBox.isValid()) { const QPointF c = s.leftBox.center(); p.setPen(QPen(QColor(255,205,0), w)); p.drawLine(c + QPointF(-w*4.0, 0), c + QPointF(w*4.0, 0)); p.drawLine(c + QPointF(0, -w*4.0), c + QPointF(0, w*4.0)); p.setPen(QPen(Qt::white, w)); p.drawText(s.leftBox.topLeft() + QPointF(4, -8), QStringLiteral("C=(%1,%2,%3)mm %4") .arg(s.center.x, 0, 'f', 1).arg(s.center.y, 0, 'f', 1) .arg(s.center.z, 0, 'f', 1).arg(s.status)); } for (const QPointF& pt : s.leftPoints) { p.setBrush(QColor(0,120,255)); p.setPen(Qt::NoPen); p.drawEllipse(pt, w*2.0, w*2.0); } p.setBrush(Qt::NoBrush); if (s.directionValid && !s.leftAxisA.isNull() && !s.leftAxisB.isNull()) { QPen axisPen(s.axisObserved ? QColor(30,120,255) : QColor(170,70,220), w); if (!s.axisObserved) axisPen.setStyle(Qt::DashLine); p.setPen(axisPen); p.drawLine(s.leftAxisA, s.leftAxisB); } } return out; } void GroovePanel::onRunPoseLocalization() { if (!m_occupancyLoaded) return; QString leftSource, rightSource; const QImage left = captureLeft(&leftSource), right = captureRight(&rightSource); if (left.isNull() || right.isNull()) { emit logMessage(QString::fromUtf8("[凹槽定位] 左右相机采集失败。")); return; } if (!ensureCalibration()) { emit logMessage(QString::fromUtf8("[凹槽定位] 双目标定参数未就绪。")); return; } const QString relPose = defaultCalibPath(QStringLiteral("pos_right2left.dat")); const auto dl = detectOccupancy(left), dr = detectOccupancy(right); QVector emptyL, emptyR; for (const auto& d : dl) if (d.classId == 0) emptyL.append(d); for (const auto& d : dr) if (d.classId == 0) emptyR.append(d); std::sort(emptyL.begin(), emptyL.end(), [](const auto& a, const auto& b) { const QPointF ca = a.box.center(), cb = b.box.center(); return ca.y() == cb.y() ? ca.x() < cb.x() : ca.y() < cb.y(); }); struct CenterCandidate { int leftIndex = -1; int rightIndex = -1; cv::Point3d centerMm; double rayMm = std::numeric_limits::infinity(); }; QVector candidates; for (int i = 0; i < emptyL.size(); ++i) { for (int j = 0; j < emptyR.size(); ++j) { const QPointF lc = emptyL[i].box.center(); const QPointF rc = emptyR[j].box.center(); cv::Point3d xyzMeters; double rayMm = std::numeric_limits::infinity(); if (!m_matcher->triangulateStereoPoint(lc.y(), lc.x(), rc.y(), rc.x(), relPose, xyzMeters, rayMm)) continue; if (rayMm > 2.0 || xyzMeters.z < 0.05 || xyzMeters.z > 2.0) continue; CenterCandidate candidate; candidate.leftIndex = i; candidate.rightIndex = j; candidate.centerMm = xyzMeters * 1000.0; candidate.rayMm = rayMm; candidates.append(candidate); } } std::sort(candidates.begin(), candidates.end(), [](const auto& a, const auto& b) { return a.rayMm < b.rayMm; }); QVector matchForLeft(emptyL.size(), -1); QVector selectedForLeft(emptyL.size()); QVector usedRight(emptyR.size(), false); for (const auto& candidate : candidates) { if (matchForLeft[candidate.leftIndex] >= 0 || usedRight[candidate.rightIndex]) continue; matchForLeft[candidate.leftIndex] = candidate.rightIndex; selectedForLeft[candidate.leftIndex] = candidate; usedRight[candidate.rightIndex] = true; } QVector poseSlots; poseSlots.reserve(emptyL.size()); for (int i = 0; i < emptyL.size(); ++i) { PoseSlot s; s.leftBox = emptyL[i].box; const int rightIndex = matchForLeft[i]; if (rightIndex < 0) { poseSlots.append(s); continue; } s.rightBox = emptyR[rightIndex].box; s.center = selectedForLeft[i].centerMm; s.centerRayErrorMm = selectedForLeft[i].rayMm; s.positionValid = true; s.status = QStringLiteral("POSITION_ONLY"); if (m_poseLoaded) { double leftPoseConfidence = 0.0, rightPoseConfidence = 0.0; double leftPoseIou = 0.0, rightPoseIou = 0.0; const bool leftPoseOk = inferFourPoints(left, s.leftBox, s.leftPoints, &leftPoseConfidence, &leftPoseIou); const bool rightPoseOk = inferFourPoints(right, s.rightBox, s.rightPoints, &rightPoseConfidence, &rightPoseIou); QPointF la, lb, ra, rb; if (leftPoseOk && rightPoseOk && axisEndpoints2d(s.leftPoints, la, lb) && axisEndpoints2d(s.rightPoints, ra, rb)) { struct AxisCandidate { cv::Point3d aMm; cv::Point3d bMm; double rayA = std::numeric_limits::infinity(); double rayB = std::numeric_limits::infinity(); double score = std::numeric_limits::infinity(); bool valid = false; }; auto tryAxis = [&](const QPointF& rightA, const QPointF& rightB) { AxisCandidate result; cv::Point3d aMeters, bMeters; if (!m_matcher->triangulateStereoPoint(la.y(), la.x(), rightA.y(), rightA.x(), relPose, aMeters, result.rayA) || !m_matcher->triangulateStereoPoint(lb.y(), lb.x(), rightB.y(), rightB.x(), relPose, bMeters, result.rayB)) return result; result.aMm = aMeters * 1000.0; result.bMm = bMeters * 1000.0; const double lengthMm = norm(result.bMm - result.aMm); const double centerOffsetMm = norm((result.aMm + result.bMm) * .5 - s.center); if (result.rayA > 2.0 || result.rayB > 2.0 || lengthMm < 5.0 || lengthMm > 40.0 || centerOffsetMm > 25.0) return result; result.score = qMax(result.rayA, result.rayB) + .25 * (result.rayA + result.rayB) + .02 * centerOffsetMm; result.valid = true; return result; }; const AxisCandidate direct = tryAxis(ra, rb); const AxisCandidate swapped = tryAxis(rb, ra); const AxisCandidate bestAxis = (!direct.valid || (swapped.valid && swapped.score < direct.score)) ? swapped : direct; if (bestAxis.valid) { s.points3d = {bestAxis.aMm, bestAxis.bMm}; s.rayErrorsMm = {bestAxis.rayA, bestAxis.rayB}; s.validPointCount = 2; s.axis = normalize(bestAxis.bMm - bestAxis.aMm); s.leftAxisA = la; s.leftAxisB = lb; s.axisObserved = true; s.directionValid = true; s.status = QStringLiteral("POSITION_AXIS"); } } } poseSlots.append(s); } // 凹槽为 180 度对称结构,方向按无符号轴求同场景共识,抑制单槽抖动。 QVector observed; for (int i = 0; i < poseSlots.size(); ++i) if (poseSlots[i].axisObserved) observed.append(i); auto consensusFrom = [&](const QVector& indices) { cv::Matx33d scatter = cv::Matx33d::zeros(); for (int index : indices) { const cv::Point3d u = normalize(poseSlots[index].axis); scatter(0,0) += u.x*u.x; scatter(0,1) += u.x*u.y; scatter(0,2) += u.x*u.z; scatter(1,0) += u.y*u.x; scatter(1,1) += u.y*u.y; scatter(1,2) += u.y*u.z; scatter(2,0) += u.z*u.x; scatter(2,1) += u.z*u.y; scatter(2,2) += u.z*u.z; } cv::Vec3d v(1.0, 0.0, 0.0); if (!indices.isEmpty()) { const auto& first = poseSlots[indices.first()].axis; v = cv::Vec3d(first.x, first.y, first.z); } for (int iteration = 0; iteration < 24; ++iteration) { const cv::Vec3d next = scatter * v; const double length = cv::norm(next); if (length < 1e-12) break; v = next * (1.0 / length); } cv::Point3d axis = normalize(cv::Point3d(v[0], v[1], v[2])); const double components[3] = {std::abs(axis.x), std::abs(axis.y), std::abs(axis.z)}; const int dominant = components[1] > components[0] ? (components[2] > components[1] ? 2 : 1) : (components[2] > components[0] ? 2 : 0); const double signValue = dominant == 0 ? axis.x : dominant == 1 ? axis.y : axis.z; if (signValue < 0.0) axis *= -1.0; return axis; }; QVector axisInliers; cv::Point3d consensusAxis; if (!observed.isEmpty()) { const cv::Point3d preliminary = consensusFrom(observed); for (int index : observed) { const double dotValue = qBound(0.0, std::abs(normalize(poseSlots[index].axis).dot(preliminary)), 1.0); const double angleDeg = std::acos(dotValue) * 180.0 / 3.14159265358979323846; if (angleDeg <= 10.0) axisInliers.append(index); else { poseSlots[index].axisObserved = false; poseSlots[index].directionValid = false; poseSlots[index].status = QStringLiteral("POSITION_ONLY"); } } if (!axisInliers.isEmpty()) consensusAxis = consensusFrom(axisInliers); } if (!axisInliers.isEmpty()) { // 料盘为规则阵列,全部凹槽互相平行。即使当前只有一个凹槽的 // 方向可靠,也把它作为本场景的公共轴线方向传播给其他已定位凹槽。 QPointF imageDirection; const PoseSlot& reference = poseSlots[axisInliers.first()]; const QPointF rawDirection = reference.leftAxisB - reference.leftAxisA; const double imageLength = std::hypot(rawDirection.x(), rawDirection.y()); if (imageLength > 1e-6) imageDirection = rawDirection / imageLength; for (auto& slot : poseSlots) { if (!slot.positionValid) continue; slot.axis = consensusAxis; slot.directionValid = true; if (!slot.axisObserved && imageLength > 1e-6) { const QPointF center = slot.leftBox.center(); const double halfLength = qMax(18.0, slot.leftBox.width() * 0.35); slot.leftAxisA = center - imageDirection * halfLength; slot.leftAxisB = center + imageDirection * halfLength; } slot.status = axisInliers.size() >= 2 ? QStringLiteral("VALID_CONSENSUS") : (slot.axisObserved ? QStringLiteral("VALID_SINGLE_AXIS_SOURCE") : QStringLiteral("VALID_SHARED_SINGLE_AXIS")); } } // 利用共识中轴和不同凹槽中心之间的平面内向量估计料盘法向。 if (!axisInliers.isEmpty()) { double bestSpan = 0.0; cv::Point3d bestNormal; cv::Point3d centerMean; int centerCount = 0; for (const auto& slot : poseSlots) if (slot.positionValid) { centerMean += slot.center; ++centerCount; } if (centerCount > 0) centerMean *= 1.0 / centerCount; for (int i = 0; i < poseSlots.size(); ++i) if (poseSlots[i].positionValid) { for (int j = i + 1; j < poseSlots.size(); ++j) if (poseSlots[j].positionValid) { const cv::Point3d candidateNormal = consensusAxis.cross(poseSlots[j].center - poseSlots[i].center); const double span = norm(candidateNormal); if (span > bestSpan) { bestSpan = span; bestNormal = candidateNormal; } } } if (bestSpan > 1.0) { bestNormal = normalize(bestNormal); if (bestNormal.dot(centerMean) > 0.0) bestNormal *= -1.0; // 朝向左相机 for (auto& slot : poseSlots) { if (!slot.positionValid || !slot.directionValid) continue; slot.normal = bestNormal; slot.normalValid = true; slot.status = axisInliers.size() >= 2 ? QStringLiteral("VALID_CONSENSUS") : (slot.axisObserved ? QStringLiteral("VALID_SINGLE_AXIS_SOURCE") : QStringLiteral("VALID_SHARED_SINGLE_AXIS")); } } } fillPoseTable(poseSlots); QImage annotated = drawPose(left, poseSlots); emit resultImage(QString::fromUtf8("凹槽三维定位"), annotated); int positionValid = 0, fullValid = 0; for (const auto& s : poseSlots) { if (s.positionValid) ++positionValid; if (s.positionValid && s.directionValid && s.normalValid) ++fullValid; } m_poseStatus->setText(QString::fromUtf8("定位结果:中心有效 %1/%2,完整方向 %3;坐标系=左相机,单位=mm") .arg(positionValid).arg(poseSlots.size()).arg(fullValid)); emit logMessage(QString::fromUtf8("[凹槽定位] 完成:左/右空槽=%1/%2,中心匹配=%3,完整位姿=%4;中心来自占位框,关键点仅辅助方向。") .arg(emptyL.size()).arg(emptyR.size()).arg(positionValid).arg(fullValid)); }