708 lines
33 KiB
C++
708 lines
33 KiB
C++
#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));
|
|
}
|