Files
VisionGuide/source/GroovePanel.cpp
T

708 lines
33 KiB
C++
Raw Normal View History

2026-10-26 10:56:56 +08:00
#include "GroovePanel.h"
#include "ui_MainWindow.h"
#include "CameraManager.h"
#include <QCoreApplication>
#include <QDir>
#include <QFileDialog>
#include <QFileInfo>
#include <QGroupBox>
#include <QHBoxLayout>
#include <QHeaderView>
#include <QLabel>
#include <QPushButton>
#include <QTableWidget>
#include <QTableWidgetItem>
#include <QVBoxLayout>
#include <QElapsedTimer>
#include <QTimer>
#include <QAbstractItemView>
#include <QPainter>
#include <QPen>
#include <algorithm>
#include <cmath>
#include <limits>
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<qreal>(0.0, intersection.width()) *
qMax<qreal>(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<QPointF>& 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<double>::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<GrooveOccupancyDetector>()),
m_pose(std::make_unique<GroovePoseDetector>())
{
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<QWidget*>(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<QWidget*>(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<QWidget*>(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<GrooveDetectionResult> 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<int>(std::floor(e.left())), size.width() - 1);
int y = qBound(0, static_cast<int>(std::floor(e.top())), size.height() - 1);
int r = qBound(x + 1, static_cast<int>(std::ceil(e.right())), size.width());
int b = qBound(y + 1, static_cast<int>(std::ceil(e.bottom())), size.height());
return QRect(x, y, r - x, b - y);
}
bool GroovePanel::inferFourPoints(const QImage& image, const QRectF& box,
QVector<QPointF>& 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<GrooveDetectionResult>& 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<GrooveDetectionResult>& 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<PoseSlot>& 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<PoseSlot>& 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<GrooveDetectionResult> 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<double>::infinity();
};
QVector<CenterCandidate> 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<double>::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<int> matchForLeft(emptyL.size(), -1);
QVector<CenterCandidate> selectedForLeft(emptyL.size());
QVector<bool> 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<PoseSlot> 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<double>::infinity();
double rayB = std::numeric_limits<double>::infinity();
double score = std::numeric_limits<double>::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<int> observed;
for (int i = 0; i < poseSlots.size(); ++i)
if (poseSlots[i].axisObserved) observed.append(i);
auto consensusFrom = [&](const QVector<int>& 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<int> 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));
}