// ════════════════════════════════════════════════════════════════════════════ // VisionGuideMatcher.cpp ─ VisionGuide 3D Shape-Based Matching 封装实现 (v4) // // [v3] 左右相机分别加载标定参数和轮廓库(CameraSide) // [v4] 新增 matchMultiple() — 全图多目标匹配, 无ROI, 无分割 // ════════════════════════════════════════════════════════════════════════════ #include "UserVisibleLog.h" #include "VisionGuideMatcher.h" #define NOMINMAX #include #include #include #include #include #include #include #include using namespace VisionGuideApi; struct VisionGuideMatcher::Impl { HTuple camParam[2]; // [CAM_LEFT, CAM_RIGHT] HTuple shapeModelId[2]; // [CAM_LEFT, CAM_RIGHT] bool camLoaded[2] = { false, false }; bool modelLoaded[2] = { false, false }; static HObject matToHImage(const cv::Mat& gray) { CV_Assert(gray.type() == CV_8UC1 && gray.isContinuous()); HObject himg; GenImage1(&himg, "byte", gray.cols, gray.rows, (Hlong)gray.data); return himg; } static std::vector> extractContours(const HObject& xldContours) { std::vector> result; HTuple numContours; CountObj(xldContours, &numContours); for (int i = 1; i <= numContours.I(); ++i) { HObject one; SelectObj(xldContours, &one, i); HTuple rows, cols; GetContourXld(one, &rows, &cols); std::vector pts; pts.reserve(rows.Length()); for (int k = 0; k < rows.Length(); ++k) pts.emplace_back(cols[k].D(), rows[k].D()); result.push_back(std::move(pts)); } return result; } static double percentileU8(const cv::Mat& gray, double q) { int hist[256] = { 0 }; int total = 0; for (int y = 0; y < gray.rows; ++y) { const uchar* row = gray.ptr(y); for (int x = 0; x < gray.cols; ++x) { ++hist[row[x]]; ++total; } } if (total <= 0) return 0.0; const int target = std::max(0, std::min(total - 1, int(std::floor(q * (total - 1))))); int acc = 0; for (int v = 0; v < 256; ++v) { acc += hist[v]; if (acc > target) return double(v); } return 255.0; } static double percentileVector(std::vector values, double q) { if (values.empty()) return 0.0; std::sort(values.begin(), values.end()); const double pos = q * double(values.size() - 1); const int lo = int(std::floor(pos)); const int hi = std::min(lo + 1, int(values.size() - 1)); const double a = pos - double(lo); return values[lo] * (1.0 - a) + values[hi] * a; } static cv::Mat makeSegmentationOverlay(const cv::Mat& grayFull, const QRect& roi, const cv::Mat& domainMask, double areaRatio, bool segOk) { cv::Mat out; if (grayFull.channels() == 1) cv::cvtColor(grayFull, out, cv::COLOR_GRAY2BGR); else out = grayFull.clone(); const cv::Rect rc(std::max(0, roi.x()), std::max(0, roi.y()), std::max(0, std::min(grayFull.cols - roi.x(), roi.width())), std::max(0, std::min(grayFull.rows - roi.y(), roi.height()))); if (rc.width > 0 && rc.height > 0) cv::rectangle(out, rc, cv::Scalar(0, 165, 255), 2, cv::LINE_AA); if (segOk && !domainMask.empty()) { cv::Mat contoursMask; if (domainMask.size() == grayFull.size()) contoursMask = domainMask; else contoursMask = cv::Mat(); if (!contoursMask.empty()) { cv::Mat overlay = out.clone(); overlay.setTo(cv::Scalar(0, 210, 60), contoursMask); cv::addWeighted(overlay, 0.26, out, 0.74, 0.0, out); std::vector> contours; cv::findContours(contoursMask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); cv::drawContours(out, contours, -1, cv::Scalar(0, 255, 0), 3, cv::LINE_AA); } cv::putText(out, cv::format("Seg OK Area=%.1f%%", areaRatio * 100.0), cv::Point(std::max(12, roi.x()), std::max(32, roi.y() - 12)), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 255, 0), 2, cv::LINE_AA); } else { cv::putText(out, "Seg FAILED use ROI", cv::Point(std::max(12, roi.x()), std::max(32, roi.y() - 12)), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); } return out; } static double projectedInsideMaskRatio(const VisionPoseResult& result, const cv::Mat& mask) { if (mask.empty()) return 0.0; int inside = 0; int total = 0; for (const auto& poly : result.projectedContours) { for (size_t i = 0; i < poly.size(); i += 3) { const int x = cvRound(poly[i].x); const int y = cvRound(poly[i].y); if (x < 0 || y < 0 || x >= mask.cols || y >= mask.rows) { ++total; continue; } ++total; if (mask.at(y, x) > 0) ++inside; } } if (total <= 0) return 0.0; return double(inside) / double(total); } bool autoSegmentBrightMetal(const cv::Mat& grayFull, const QRect& roi, const HObject& roiRegion, int segDilation, HObject& finalDomain, double& areaRatio, double& segCenterRow, double& segCenterCol, cv::Mat* outDomainMask = nullptr) { if (grayFull.empty() || grayFull.type() != CV_8UC1) return false; const int x1 = std::max(0, roi.x()); const int y1 = std::max(0, roi.y()); const int x2 = std::min(grayFull.cols - 1, roi.x() + roi.width() - 1); const int y2 = std::min(grayFull.rows - 1, roi.y() + roi.height() - 1); if (x2 <= x1 || y2 <= y1) return false; const cv::Rect rc(x1, y1, x2 - x1 + 1, y2 - y1 + 1); cv::Mat crop = grayFull(rc); const int cropArea = std::max(1, rc.area()); cv::Mat blur, eq; cv::GaussianBlur(crop, blur, cv::Size(3, 3), 0.0); cv::Ptr clahe = cv::createCLAHE(2.0, cv::Size(8, 8)); clahe->apply(blur, eq); const double p95 = percentileU8(eq, 0.95); const double p985 = percentileU8(eq, 0.985); double brightThr = std::max(178.0, p985); cv::Mat seed; cv::threshold(eq, seed, brightThr - 1.0, 255.0, cv::THRESH_BINARY); const int minSeedPixels = std::max(35, int(cropArea * 0.00035)); if (cv::countNonZero(seed) < minSeedPixels) { brightThr = std::max(145.0, p95); cv::threshold(eq, seed, brightThr - 1.0, 255.0, cv::THRESH_BINARY); } cv::morphologyEx(seed, seed, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3))); cv::Mat labels, stats, centroids; int n = cv::connectedComponentsWithStats(seed > 0, labels, stats, centroids, 8, CV_32S); cv::Mat cleanSeed(seed.size(), CV_8UC1, cv::Scalar(0)); const double roiCx = rc.width * 0.5; const double roiCy = rc.height * 0.5; for (int i = 1; i < n; ++i) { const int area = stats.at(i, cv::CC_STAT_AREA); if (area < std::max(8, minSeedPixels / 6)) continue; const double dx = (centroids.at(i, 0) - roiCx) / std::max(1, rc.width); const double dy = (centroids.at(i, 1) - roiCy) / std::max(1, rc.height); if (dx * dx + dy * dy > 0.42) continue; cleanSeed.setTo(255, labels == i); } if (cv::countNonZero(cleanSeed) < minSeedPixels) cleanSeed = seed; std::vector pts; pts.reserve(cv::countNonZero(cleanSeed)); for (int y = 0; y < cleanSeed.rows; ++y) { const uchar* row = cleanSeed.ptr(y); for (int x = 0; x < cleanSeed.cols; ++x) { if (row[x]) pts.emplace_back(double(x), double(y)); } } if (int(pts.size()) < minSeedPixels) return false; cv::Mat data(int(pts.size()), 2, CV_64F); for (int i = 0; i < int(pts.size()); ++i) { data.at(i, 0) = pts[i].x; data.at(i, 1) = pts[i].y; } cv::PCA pca(data, cv::Mat(), cv::PCA::DATA_AS_ROW); cv::Point2d center(pca.mean.at(0, 0), pca.mean.at(0, 1)); cv::Point2d axis(pca.eigenvectors.at(0, 0), pca.eigenvectors.at(0, 1)); const double axisNorm = std::sqrt(axis.x * axis.x + axis.y * axis.y); if (axisNorm < 1e-6) return false; axis.x /= axisNorm; axis.y /= axisNorm; cv::Point2d perp(-axis.y, axis.x); std::vector longVals; std::vector perpVals; longVals.reserve(pts.size()); perpVals.reserve(pts.size()); for (const auto& p : pts) { const cv::Point2d d = p - center; longVals.push_back(d.x * axis.x + d.y * axis.y); perpVals.push_back(d.x * perp.x + d.y * perp.y); } const double longSpan = percentileVector(longVals, 0.985) - percentileVector(longVals, 0.015); const double perpSpan = percentileVector(perpVals, 0.985) - percentileVector(perpVals, 0.015); const double minDim = double(std::min(rc.width, rc.height)); const double diag = std::sqrt(double(rc.width) * rc.width + double(rc.height) * rc.height); double longHalf = std::max(longSpan * 0.78 + 55.0, minDim * 0.34); longHalf = std::min(longHalf, diag * 0.50); double widthHalf = std::max({ perpSpan * 3.8 + 45.0, longHalf * 0.28, minDim * 0.22 }); widthHalf = std::min(widthHalf, minDim * 0.46); longHalf += std::max(8, segDilation); widthHalf += std::max(8, segDilation); const cv::Point2d localCorners[4] = { center + axis * longHalf + perp * widthHalf, center + axis * longHalf - perp * widthHalf, center - axis * longHalf - perp * widthHalf, center - axis * longHalf + perp * widthHalf }; std::vector corners; corners.reserve(4); for (const auto& c : localCorners) { corners.emplace_back(cvRound(c.x + x1), cvRound(c.y + y1)); } cv::Mat globalMask(grayFull.size(), CV_8UC1, cv::Scalar(0)); cv::fillConvexPoly(globalMask, corners, cv::Scalar(255), cv::LINE_AA); cv::Mat roiOnly(grayFull.size(), CV_8UC1, cv::Scalar(0)); cv::rectangle(roiOnly, rc, cv::Scalar(255), cv::FILLED); cv::bitwise_and(globalMask, roiOnly, globalMask); if (!globalMask.isContinuous()) globalMask = globalMask.clone(); if (outDomainMask) *outDomainMask = globalMask.clone(); try { HObject hmask, region, intersected; hmask = matToHImage(globalMask); Threshold(hmask, ®ion, 1, 255); Intersection(region, roiRegion, &intersected); HTuple area, cy, cx, roiArea, roiCy, roiCx; AreaCenter(intersected, &area, &cy, &cx); AreaCenter(roiRegion, &roiArea, &roiCy, &roiCx); if (area.Length() == 0 || roiArea.D() <= 1.0) return false; areaRatio = area.D() / roiArea.D(); if (areaRatio <= 0.015 || areaRatio >= 0.75) return false; segCenterRow = cy.D(); segCenterCol = cx.D(); finalDomain = intersected; return true; } catch (HException&) { return false; } } bool autoSegment(const HObject& himg, const HObject& roiRegion, int segDilation, HObject& finalDomain, double& areaRatio, double& segCenterRow, double& segCenterCol) { try { HObject imgInRoi, darkRegion, connComps, candidate, filled; HTuple threshold, numComps, area, cy, cx; ReduceDomain(himg, roiRegion, &imgInRoi); BinaryThreshold(imgInRoi, &darkRegion, "max_separability", "dark", &threshold); Connection(darkRegion, &connComps); CountObj(connComps, &numComps); if (numComps.I() > 0) { SelectShapeStd(connComps, &candidate, "max_area", 70); HTuple cntFiltered; CountObj(candidate, &cntFiltered); if (cntFiltered.I() == 0) { finalDomain = roiRegion; areaRatio = 0; segCenterRow = segCenterCol = 0; return false; } FillUp(candidate, &filled); AreaCenter(filled, &area, &cy, &cx); HTuple roiArea, roiCy, roiCx; AreaCenter(roiRegion, &roiArea, &roiCy, &roiCx); areaRatio = area.D() / roiArea.D(); segCenterRow = cy.D(); segCenterCol = cx.D(); if (areaRatio > 0.05 && areaRatio < 0.70) { HObject dilated, intersected; DilationCircle(filled, &dilated, segDilation); Intersection(dilated, roiRegion, &intersected); finalDomain = intersected; return true; } } } catch (HException&) {} finalDomain = roiRegion; areaRatio = 0; segCenterRow = segCenterCol = 0; return false; } // ── 单目标匹配内核 ────────────────────────────────────── bool findMatch(const HObject& imgMedian, CameraSide side, double minScore, double greediness, const QString& poseRefinement, HTuple& outPose, HTuple& outCovPose, HTuple& outScore, double& matchTimeMs) { QElapsedTimer timer; timer.start(); HTuple genNames, genValues; genNames = HTuple("num_matches").TupleConcat("max_overlap") .TupleConcat("pose_refinement").TupleConcat("recompute_score") .TupleConcat("border_model").TupleConcat("cov_pose_mode"); genValues = HTuple(1).TupleConcat(0.0) .TupleConcat(poseRefinement.toLatin1().constData()) .TupleConcat("true").TupleConcat("false").TupleConcat("standard_deviations"); FindShapeModel3d(imgMedian, shapeModelId[side], minScore, greediness, 0, genNames, genValues, &outPose, &outCovPose, &outScore); matchTimeMs = timer.elapsed(); return outScore.Length() > 0; } // ── 多目标匹配内核 (指标测量用) ───────────────────────── bool findMatchMultiple(const HObject& imgReady, CameraSide side, int numMatches, double minScore, double greediness, const QString& poseRefinement, HTuple& outPose, HTuple& outCovPose, HTuple& outScore, double& matchTimeMs) { QElapsedTimer timer; timer.start(); HTuple genNames, genValues; genNames.Clear(); genValues.Clear(); genNames.Append("num_matches"); genValues.Append(numMatches); if (numMatches > 1) { genNames.Append("max_overlap"); genValues.Append(0.3); } genNames.Append("pose_refinement"); genValues.Append(poseRefinement.isEmpty() ? "least_squares" : poseRefinement.toLatin1().constData()); genNames.Append("recompute_score"); genValues.Append("true"); genNames.Append("border_model"); genValues.Append("false"); /* genNames = HTuple("num_matches") .TupleConcat("max_overlap") .TupleConcat("pose_refinement") .TupleConcat("recompute_score") .TupleConcat("border_model"); genValues = HTuple(numMatches) .TupleConcat(0.3) // 允许少量重叠 .TupleConcat("least_squares_very_high") // 最高精度细化 .TupleConcat("true") .TupleConcat("true") .TupleConcat("false"); */ FindShapeModel3d(imgReady, shapeModelId[side], minScore, greediness, 0, genNames, genValues, &outPose, &outCovPose, &outScore); matchTimeMs = timer.elapsed(); return outScore.Length() > 0; } // ── 解析单个位姿 (从 pose tuple 的偏移位置) ────────────── void parseSinglePose(const HTuple& allPose, int idx, const HTuple& allCovPose, const HTuple& allScore, double coordAxisLen, CameraSide side, VisionPoseResult& result, bool projectContours = true) { int off = idx * 7; result.tx = allPose[off + 0].D(); result.ty = allPose[off + 1].D(); result.tz = allPose[off + 2].D(); result.rx = allPose[off + 3].D(); result.ry = allPose[off + 4].D(); result.rz = allPose[off + 5].D(); result.poseType = allPose[off + 6].I(); result.score = allScore[idx].D(); HTuple bestPose; bestPose.Clear(); for (int i = 0; i < 7; ++i) bestPose.Append(allPose[off + i]); // 投影轮廓 if (projectContours) { HObject projContours; ProjectShapeModel3d(&projContours, shapeModelId[side], camParam[side], bestPose, "true", HTuple(30).TupleRad()); result.projectedContours = extractContours(projContours); } // 坐标轴投影 HTuple hmat; PoseToHomMat3d(bestPose, &hmat); HTuple oRow, oCol; Project3dPoint(result.tx, result.ty, result.tz, camParam[side], &oRow, &oCol); result.originRow = oRow.D(); result.originCol = oCol.D(); auto projectTip = [&](double dx, double dy, double dz, double& tipRow, double& tipCol) { HTuple X, Y, Z, r, c; AffineTransPoint3d(hmat, dx, dy, dz, &X, &Y, &Z); Project3dPoint(X, Y, Z, camParam[side], &r, &c); tipRow = r.D(); tipCol = c.D(); }; double L = coordAxisLen; projectTip(L, 0, 0, result.xTipRow, result.xTipCol); projectTip(0, L, 0, result.yTipRow, result.yTipCol); projectTip(0, 0, L, result.zTipRow, result.zTipCol); // 协方差 (每个匹配6个分量) int covOff = idx * 6; if (allCovPose.Length() >= covOff + 6) { // 视觉引导's default CovPose contains standard deviations, not variances. result.stdTx = std::abs(allCovPose[covOff + 0].D()) * 1000.0; result.stdTy = std::abs(allCovPose[covOff + 1].D()) * 1000.0; result.stdTz = std::abs(allCovPose[covOff + 2].D()) * 1000.0; result.stdRx = std::abs(allCovPose[covOff + 3].D()); result.stdRy = std::abs(allCovPose[covOff + 4].D()); result.stdRz = std::abs(allCovPose[covOff + 5].D()); } result.valid = true; } // ── 旧版单目标解析 (兼容) ──────────────────────────────── void parsePose(const HTuple& pose, const HTuple& covPose, const HTuple& score, double matchTimeMs, double coordAxisLen, CameraSide side, VisionPoseResult& result) { parseSinglePose(pose, 0, covPose, score, coordAxisLen, side, result); result.matchTimeMs = matchTimeMs; } }; // ═══════════════════════════════════════════════════════════════ VisionGuideMatcher::VisionGuideMatcher(QObject* parent) : QObject(parent), m_d(std::make_unique()) {} VisionGuideMatcher::~VisionGuideMatcher() { if (m_d) { for (int s = 0; s < 2; ++s) { if (m_d->modelLoaded[s]) { try { ClearShapeModel3d(m_d->shapeModelId[s]); } catch (...) {} } } } } bool VisionGuideMatcher::isReady(CameraSide side) const { return m_d && m_d->camLoaded[side] && m_d->modelLoaded[side]; } bool VisionGuideMatcher::isCamLoaded(CameraSide side) const { return m_d && m_d->camLoaded[side]; } bool VisionGuideMatcher::isModelLoaded(CameraSide side) const { return m_d && m_d->modelLoaded[side]; } QString VisionGuideMatcher::cameraParamFile(CameraSide side) const { return m_camParFile[side]; } QString VisionGuideMatcher::shapeModelFile(CameraSide side) const { return m_modelFile[side]; } bool VisionGuideMatcher::loadCameraParams(const QString& calFile, CameraSide side) { try { QFile file(calFile); if (!file.open(QIODevice::ReadOnly)) { m_d->camLoaded[side] = false; m_camParSha256[side].clear(); emit logMessage(QString::fromUtf8("[VisionGuide] 无法读取相机参数文件: %1").arg(calFile)); return false; } QByteArray bytes = file.readAll(); ReadCamPar(HTuple(calFile.toLocal8Bit().constData()), &m_d->camParam[side]); m_d->camLoaded[side] = true; m_camParFile[side] = calFile; m_camParSha256[side] = QString::fromLatin1(QCryptographicHash::hash(bytes, QCryptographicHash::Sha256).toHex()); QString s = (side == CAM_LEFT) ? QString::fromUtf8("左") : QString::fromUtf8("右"); emit logMessage(QString::fromUtf8("[VisionGuide] %1相机参数加载成功: %2").arg(s, calFile)); return true; } catch (HException& e) { m_d->camLoaded[side] = false; emit logMessage(QString::fromUtf8("[VisionGuide] 相机参数加载失败: %1") .arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())))); return false; } } bool VisionGuideMatcher::loadShapeModel(const QString& sm3File, CameraSide side) { try { if (m_d->modelLoaded[side]) { ClearShapeModel3d(m_d->shapeModelId[side]); m_d->modelLoaded[side] = false; } ReadShapeModel3d(HTuple(sm3File.toLocal8Bit().constData()), &m_d->shapeModelId[side]); m_d->modelLoaded[side] = true; m_modelFile[side] = sm3File; QString s = (side == CAM_LEFT) ? QString::fromUtf8("左") : QString::fromUtf8("右"); emit logMessage(QString::fromUtf8("[VisionGuide] %1相机3D形状模板加载成功: %2").arg(s, sm3File)); return true; } catch (HException& e) { m_d->modelLoaded[side] = false; emit logMessage(QString::fromUtf8("[VisionGuide] 模板加载失败: %1") .arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())))); return false; } } bool VisionGuideMatcher::triangulateStereoPoint(double leftRow, double leftCol, double rightRow, double rightCol, const QString& rightToLeftPoseFile, cv::Point3d& pointLeftMeters, double& rayDistanceMm) { pointLeftMeters = cv::Point3d(); rayDistanceMm = std::numeric_limits::infinity(); if (!m_d || !m_d->camLoaded[CAM_LEFT] || !m_d->camLoaded[CAM_RIGHT] || rightToLeftPoseFile.isEmpty()) { return false; } try { HTuple relPose; ReadPose(HTuple(rightToLeftPoseFile.toLocal8Bit().constData()), &relPose); HTuple x, y, z, dist; IntersectLinesOfSight(m_d->camParam[CAM_LEFT], m_d->camParam[CAM_RIGHT], relPose, leftRow, leftCol, rightRow, rightCol, &x, &y, &z, &dist); if (x.Length() <= 0 || y.Length() <= 0 || z.Length() <= 0) return false; pointLeftMeters = cv::Point3d(x[0].D(), y[0].D(), z[0].D()); rayDistanceMm = (dist.Length() > 0) ? dist[0].D() * 1000.0 : 0.0; return std::isfinite(pointLeftMeters.x) && std::isfinite(pointLeftMeters.y) && std::isfinite(pointLeftMeters.z) && std::isfinite(rayDistanceMm); } catch (HException& e) { emit logMessage(QStringLiteral("[Triangulate] VisionGuide error: %1") .arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())))); return false; } } bool VisionGuideMatcher::predictRightRoi(const QRect& leftRoi, const QString& rightToLeftPoseFile, const QSize& rightImageSize, QRect& rightRoi, double estimatedDepthMeters, double paddingRatio) const { rightRoi = QRect(); if (!m_d || !m_d->camLoaded[CAM_LEFT] || !m_d->camLoaded[CAM_RIGHT] || !leftRoi.isValid() || rightToLeftPoseFile.isEmpty() || rightImageSize.width() <= 0 || rightImageSize.height() <= 0) { return false; } try { HTuple poseR2L, hmatR2L, hmatL2R; ReadPose(HTuple(rightToLeftPoseFile.toLocal8Bit().constData()), &poseR2L); PoseToHomMat3d(poseR2L, &hmatR2L); HomMat3dInvert(hmatR2L, &hmatL2R); const double depth = std::max(0.05, std::min(2.0, estimatedDepthMeters)); const double depthMargin = std::max(0.015, depth * 0.08); const double depths[2] = { std::max(0.03, depth - depthMargin), depth + depthMargin }; const QPointF corners[4] = { QPointF(leftRoi.left(), leftRoi.top()), QPointF(leftRoi.right(), leftRoi.top()), QPointF(leftRoi.right(), leftRoi.bottom()), QPointF(leftRoi.left(), leftRoi.bottom()) }; double minCol = std::numeric_limits::infinity(); double minRow = std::numeric_limits::infinity(); double maxCol = -std::numeric_limits::infinity(); double maxRow = -std::numeric_limits::infinity(); int projectedCount = 0; for (double zTarget : depths) { for (const QPointF& imagePoint : corners) { HTuple px, py, pz, qx, qy, qz; GetLineOfSight(imagePoint.y(), imagePoint.x(), m_d->camParam[CAM_LEFT], &px, &py, &pz, &qx, &qy, &qz); if (px.Length() <= 0 || qx.Length() <= 0) continue; const double dz = qz[0].D() - pz[0].D(); if (std::abs(dz) < 1e-12) continue; const double scale = (zTarget - pz[0].D()) / dz; const double xLeft = px[0].D() + scale * (qx[0].D() - px[0].D()); const double yLeft = py[0].D() + scale * (qy[0].D() - py[0].D()); const double zLeft = pz[0].D() + scale * dz; HTuple xRight, yRight, zRight; AffineTransPoint3d(hmatL2R, xLeft, yLeft, zLeft, &xRight, &yRight, &zRight); if (zRight.Length() <= 0 || zRight[0].D() <= 1e-6) continue; HTuple rowRight, colRight; Project3dPoint(xRight, yRight, zRight, m_d->camParam[CAM_RIGHT], &rowRight, &colRight); if (rowRight.Length() <= 0 || colRight.Length() <= 0) continue; const double row = rowRight[0].D(); const double col = colRight[0].D(); if (!std::isfinite(row) || !std::isfinite(col)) continue; minCol = std::min(minCol, col); maxCol = std::max(maxCol, col); minRow = std::min(minRow, row); maxRow = std::max(maxRow, row); ++projectedCount; } } if (projectedCount < 4 || maxCol <= minCol || maxRow <= minRow) return false; const double padX = std::max(24.0, (maxCol - minCol) * paddingRatio); const double padY = std::max(24.0, (maxRow - minRow) * paddingRatio); QRectF projected(minCol - padX, minRow - padY, maxCol - minCol + 2.0 * padX, maxRow - minRow + 2.0 * padY); const QRect bounds(QPoint(0, 0), rightImageSize); rightRoi = projected.toAlignedRect().intersected(bounds); return rightRoi.width() >= 10 && rightRoi.height() >= 10; } catch (HException&) { return false; } } cv::Mat VisionGuideMatcher::applyPreFilter(const cv::Mat& grayIn) const { if (!params.useFilter) return grayIn.clone(); int ksize = params.filterKernelSize; if (ksize < 1) ksize = 1; if (ksize % 2 == 0) ksize += 1; cv::Mat out; switch (params.filterType) { case Filter_Gaussian: cv::GaussianBlur(grayIn, out, cv::Size(ksize, ksize), params.filterSigma, params.filterSigma, cv::BORDER_REFLECT); break; case Filter_Median: cv::medianBlur(grayIn, out, ksize); break; case Filter_Bilateral: cv::bilateralFilter(grayIn, out, ksize, params.filterSigmaColor, params.filterSigmaSpace, cv::BORDER_REFLECT); break; default: out = grayIn.clone(); break; } return out; } // ════════════════════════════════════════════════════════════════════════════ // 单目标匹配 (带 ROI + 可选自动分割) — 原有流程 // ════════════════════════════════════════════════════════════════════════════ VisionPoseResult VisionGuideMatcher::match(const cv::Mat& grayFull, const QRect& roi, CameraSide side, bool fastPreprocess, const Params* runParams) { VisionPoseResult result; const Params active = runParams ? *runParams : params; QElapsedTimer prepTimer; prepTimer.start(); if (!isReady(side)) { QString s = (side == CAM_LEFT) ? QString::fromUtf8("左") : QString::fromUtf8("右"); result.errorMsg = QString::fromUtf8("%1相机参数或模板未加载").arg(s); return result; } if (grayFull.empty() || grayFull.type() != CV_8UC1) { result.errorMsg = QString::fromUtf8("输入图像必须是单通道灰度图"); return result; } try { emit progressUpdated(10, QString::fromUtf8("准备图像...")); // 快速模式不再对整张 5120x5120 图做 OpenCV 滤波;先建立图像, // ReduceDomain 后仅在 ROI 域执行轻量中值滤波。 cv::Mat gray; if (fastPreprocess || !active.useFilter) gray = grayFull; else { int kernel = std::max(1, active.filterKernelSize); if (!(kernel % 2)) ++kernel; if (active.filterType == Filter_Median) cv::medianBlur(grayFull, gray, kernel); else if (active.filterType == Filter_Bilateral) cv::bilateralFilter(grayFull, gray, kernel, active.filterSigmaColor, active.filterSigmaSpace, cv::BORDER_REFLECT); else cv::GaussianBlur(grayFull, gray, cv::Size(kernel,kernel), active.filterSigma, active.filterSigma, cv::BORDER_REFLECT); } if (!gray.isContinuous()) gray = gray.clone(); HObject himg = Impl::matToHImage(gray); HObject roiRegion; GenRectangle1(&roiRegion, roi.top(), roi.left(), roi.top() + roi.height() - 1, roi.left() + roi.width() - 1); const bool useAutoSeg = active.useAutoSeg && !fastPreprocess; emit progressUpdated(20, useAutoSeg ? QString::fromUtf8("自动分割...") : QString::fromUtf8("跳过自动分割...")); HObject finalDomain; cv::Mat segmentMask; if (useAutoSeg) { result.segmentationOk = m_d->autoSegmentBrightMetal( gray, roi, roiRegion, active.segDilation, finalDomain, result.segAreaRatio, result.segCenterRow, result.segCenterCol, &segmentMask); if (!result.segmentationOk) { result.segmentationOk = m_d->autoSegment( himg, roiRegion, active.segDilation, finalDomain, result.segAreaRatio, result.segCenterRow, result.segCenterCol); } if (!result.segmentationOk) finalDomain = roiRegion; } else { finalDomain = roiRegion; } if (useAutoSeg && active.generateDisplayContours) { result.segmentationDebugImage = Impl::makeSegmentationOverlay( gray, roi, segmentMask, result.segAreaRatio, result.segmentationOk); } else { result.segmentationDebugImage.release(); } emit progressUpdated(40, QString::fromUtf8("预处理...")); HObject imgForMatch, imgMedian; ReduceDomain(himg, finalDomain, &imgForMatch); if (fastPreprocess) MedianImage(imgForMatch, &imgMedian, "circle", 3, "mirrored"); else if (!active.useFilter) MedianImage(imgForMatch, &imgMedian, "circle", 3, "mirrored"); else imgMedian = imgForMatch; emit progressUpdated(60, QString::fromUtf8("3D形状匹配中...")); HTuple pose, covPose, score; double matchTimeMs = 0; result.preprocessTimeMs = prepTimer.elapsed(); bool found = m_d->findMatch(imgMedian, side, active.minScore, active.greediness, active.poseRefinement, pose, covPose, score, matchTimeMs); result.matchTimeMs = matchTimeMs; if (!found) { result.errorMsg = QString::fromUtf8("未找到匹配"); emit progressUpdated(100, QString::fromUtf8("匹配失败")); return result; } emit progressUpdated(80, QString::fromUtf8("解析位姿...")); QElapsedTimer parseTimer; parseTimer.start(); const bool needsQualityProjection = useAutoSeg && result.segmentationOk && !segmentMask.empty(); m_d->parseSinglePose(pose, 0, covPose, score, active.coordAxisLen_m, side, result, active.generateDisplayContours || needsQualityProjection); result.parseTimeMs = parseTimer.elapsed(); if (useAutoSeg && result.segmentationOk) { if (!segmentMask.empty()) { result.projectedInsideSegRatio = Impl::projectedInsideMaskRatio(result, segmentMask); if (result.projectedInsideSegRatio < 0.78) { result.valid = false; result.errorMsg = QString::fromUtf8("投影轮廓落在自动分割域内比例过低: %1%") .arg(result.projectedInsideSegRatio * 100.0, 0, 'f', 1); emit logMessage(QString::fromUtf8("[VisionGuide] reject pose: %1").arg(result.errorMsg)); emit progressUpdated(100, QString::fromUtf8("匹配被分割域校验拒绝")); return result; } } double dr = result.originRow - result.segCenterRow; double dc = result.originCol - result.segCenterCol; result.distError = std::sqrt(dr*dr + dc*dc); const double maxCenterDrift = std::max(55.0, 0.22 * std::max(roi.width(), roi.height())); if (result.distError > maxCenterDrift) { result.valid = false; result.errorMsg = QString::fromUtf8("匹配中心偏离自动分割区域过远: %1px > %2px") .arg(result.distError, 0, 'f', 1) .arg(maxCenterDrift, 0, 'f', 1); emit logMessage(QString::fromUtf8("[VisionGuide] reject pose: %1").arg(result.errorMsg)); emit progressUpdated(100, QString::fromUtf8("匹配被几何校验拒绝")); return result; } } emit progressUpdated(100, QString::fromUtf8("匹配完成")); QString s = (side == CAM_LEFT) ? QString::fromUtf8("左") : QString::fromUtf8("右"); emit logMessage(QString::fromUtf8("[%1路] 匹配成功 Score=%2 耗时=%3ms") .arg(s).arg(result.score, 0, 'f', 4).arg(result.matchTimeMs, 0, 'f', 0)); return result; } catch (HException& e) { result.errorMsg = QString::fromUtf8("VisionGuide异常: %1") .arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA()))); emit logMessage(result.errorMsg); return result; } } // ════════════════════════════════════════════════════════════════════════════ // 多目标匹配 (全图, 无 ROI, 无分割, 指标测量球棒用) // ════════════════════════════════════════════════════════════════════════════ std::vector VisionGuideMatcher::matchMultiple( const cv::Mat& grayFull, int numMatches, CameraSide side, const QRect& searchRoi) { std::vector results; if (!isReady(side)) { QString s = (side == CAM_LEFT) ? QString::fromUtf8("左") : QString::fromUtf8("右"); emit logMessage(QString::fromUtf8("[MultiMatch] %1相机参数或模板未加载").arg(s)); return results; } if (grayFull.empty()) { emit logMessage(QString::fromUtf8("[MultiMatch] 输入图像为空")); return results; } try { // 预处理 cv::Mat gray; if (grayFull.type() != CV_8UC1) { cv::cvtColor(grayFull, gray, cv::COLOR_BGR2GRAY); } else { gray = grayFull; } gray = applyPreFilter(gray); if (!gray.isContinuous()) gray = gray.clone(); HObject himg = Impl::matToHImage(gray); HObject imgForMatch = himg; if (searchRoi.isValid() && searchRoi.width() > 0 && searchRoi.height() > 0) { const int x1 = (std::max)(0, searchRoi.left()); const int y1 = (std::max)(0, searchRoi.top()); const int x2 = (std::min)(gray.cols - 1, searchRoi.right()); const int y2 = (std::min)(gray.rows - 1, searchRoi.bottom()); if (x2 > x1 && y2 > y1) { HObject roiRegion; GenRectangle1(&roiRegion, y1, x1, y2, x2); ReduceDomain(himg, roiRegion, &imgForMatch); } } // 全图匹配, 不做 ReduceDomain, 不做分割 HTuple pose, covPose, score; double matchTimeMs = 0; bool found = m_d->findMatchMultiple( imgForMatch, side, numMatches, params.minScore, params.greediness, params.poseRefinement, pose, covPose, score, matchTimeMs); if (!found) { emit logMessage(QString::fromUtf8("[MultiMatch] 未找到任何匹配")); return results; } int nFound = score.Length(); emit logMessage(QString::fromUtf8("[MultiMatch] 找到 %1 个匹配, 耗时 %2ms") .arg(nFound).arg(matchTimeMs, 0, 'f', 0)); // 解析每个匹配结果 for (int i = 0; i < nFound; ++i) { VisionPoseResult r; m_d->parseSinglePose(pose, i, covPose, score, params.coordAxisLen_m, side, r); r.matchTimeMs = matchTimeMs; r.segmentationOk = false; // 无分割 emit logMessage(QString::fromUtf8(" 匹配[%1] Score=%2 tx=%3 ty=%4 tz=%5 (m)") .arg(i).arg(r.score, 0, 'f', 5) .arg(r.tx, 0, 'f', 6).arg(r.ty, 0, 'f', 6).arg(r.tz, 0, 'f', 6)); results.push_back(r); } // 按 score 降序排序 std::sort(results.begin(), results.end(), [](const VisionPoseResult& a, const VisionPoseResult& b) { return a.score > b.score; }); return results; } catch (HException& e) { emit logMessage(QString::fromUtf8("[MultiMatch] VisionGuide异常: %1") .arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())))); return results; } } // ════════════════════════════════════════════════════════════════════════════ // Step 1 自动分割 — 不需要相机参数和模板 // ════════════════════════════════════════════════════════════════════════════ std::vector VisionGuideMatcher::matchMultipleInMask( const cv::Mat& grayFull, const cv::Mat& domainMask, int numMatches, CameraSide side) { std::vector results; if (!isReady(side)) { emit logMessage(QStringLiteral("[MultiMatchMask] camera/model not ready")); return results; } if (grayFull.empty() || domainMask.empty()) { emit logMessage(QStringLiteral("[MultiMatchMask] empty image or domain mask")); return results; } try { QElapsedTimer totalTimer; QElapsedTimer stepTimer; totalTimer.start(); stepTimer.start(); const QString sideName = (side == CAM_LEFT) ? QStringLiteral("L") : QStringLiteral("R"); emit logMessage(QStringLiteral("[MultiMatchMask] begin side=%1 image=%2x%3 mask=%4x%5 num=%6") .arg(sideName) .arg(grayFull.cols).arg(grayFull.rows) .arg(domainMask.cols).arg(domainMask.rows) .arg(numMatches)); cv::Mat gray; if (grayFull.type() != CV_8UC1) { cv::cvtColor(grayFull, gray, cv::COLOR_BGR2GRAY); } else { gray = grayFull; } if (domainMask.size() != gray.size()) { emit logMessage(QStringLiteral("[MultiMatchMask] mask size mismatch")); return results; } gray = applyPreFilter(gray); if (!gray.isContinuous()) gray = gray.clone(); emit logMessage(QStringLiteral("[MultiMatchMask] side=%1 filter done: %2ms") .arg(sideName).arg(stepTimer.elapsed())); stepTimer.restart(); cv::Mat mask; if (domainMask.type() != CV_8UC1) { cv::cvtColor(domainMask, mask, cv::COLOR_BGR2GRAY); } else { mask = domainMask.clone(); } cv::threshold(mask, mask, 0, 255, cv::THRESH_BINARY); if (!mask.isContinuous()) mask = mask.clone(); const int nonZero = cv::countNonZero(mask); const cv::Rect maskBox = nonZero > 0 ? cv::boundingRect(mask) : cv::Rect(); emit logMessage(QStringLiteral("[MultiMatchMask] side=%1 mask ready: nz=%2 bbox=(%3,%4,%5,%6) time=%7ms") .arg(sideName).arg(nonZero) .arg(maskBox.x).arg(maskBox.y).arg(maskBox.width).arg(maskBox.height) .arg(stepTimer.elapsed())); stepTimer.restart(); HObject himg = Impl::matToHImage(gray); HObject hmask = Impl::matToHImage(mask); HObject domainRegion, imgForMatch; Threshold(hmask, &domainRegion, 1, 255); ReduceDomain(himg, domainRegion, &imgForMatch); emit logMessage(QStringLiteral("[MultiMatchMask] side=%1 VisionGuide ReduceDomain done: %2ms") .arg(sideName).arg(stepTimer.elapsed())); HTuple pose, covPose, score; double matchTimeMs = 0.0; emit logMessage(QStringLiteral("[MultiMatchMask] side=%1 FindShapeModel3d begin minScore=%2 greed=%3 paramMode=%4") .arg(sideName) .arg(params.minScore, 0, 'f', 2) .arg(params.greediness, 0, 'f', 2) .arg(numMatches > 1 ? QStringLiteral("multi+max_overlap") : QStringLiteral("single-no-max_overlap"))); emit logMessage(QStringLiteral("[MultiMatchMask] side=%1 uses cal=%2 model=%3") .arg(sideName) .arg(m_camParFile[side]) .arg(m_modelFile[side])); const bool found = m_d->findMatchMultiple( imgForMatch, side, numMatches, params.minScore, params.greediness, params.poseRefinement, pose, covPose, score, matchTimeMs); if (!found) { emit logMessage(QStringLiteral("[MultiMatchMask] no match")); return results; } const int nFound = score.Length(); emit logMessage(QStringLiteral("[MultiMatchMask] side=%1 FindShapeModel3d end: found=%2 matchTime=%3ms total=%4ms") .arg(sideName).arg(nFound).arg(matchTimeMs, 0, 'f', 0).arg(totalTimer.elapsed())); for (int i = 0; i < nFound; ++i) { VisionPoseResult r; m_d->parseSinglePose(pose, i, covPose, score, params.coordAxisLen_m, side, r); r.matchTimeMs = matchTimeMs; r.segmentationOk = true; results.push_back(r); } std::sort(results.begin(), results.end(), [](const VisionPoseResult& a, const VisionPoseResult& b) { return a.score > b.score; }); emit logMessage(QStringLiteral("[MultiMatchMask] side=%1 parse/sort done total=%2ms") .arg(sideName).arg(totalTimer.elapsed())); return results; } catch (HException& e) { emit logMessage(QStringLiteral("[MultiMatchMask] VisionGuide error: %1") .arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())))); return results; } } bool VisionGuideMatcher::stepAutoSegment(const cv::Mat& grayFull, const QRect& roi, cv::Mat& segResultImg, double& areaRatio, bool& segOk) { areaRatio = 0; segOk = false; segResultImg.release(); if (!m_d) { emit logMessage(QStringLiteral("[Seg] Impl 未初始化")); return false; } if (grayFull.empty()) { emit logMessage(QStringLiteral("[Seg] 输入图像为空")); return false; } if (roi.isEmpty() || roi.width() <= 0 || roi.height() <= 0) { emit logMessage(QStringLiteral("[Seg] ROI 无效")); return false; } cv::Mat grayForVisionGuide = (grayFull.channels() == 1) ? grayFull : [&] { cv::Mat g; cv::cvtColor(grayFull, g, cv::COLOR_BGR2GRAY); return g; }(); try { cv::Mat gray = applyPreFilter(grayForVisionGuide); if (!gray.isContinuous()) gray = gray.clone(); HObject himg = Impl::matToHImage(gray); int x1 = std::max(0, roi.x()); int y1 = std::max(0, roi.y()); int x2 = std::min(grayForVisionGuide.cols - 1, roi.x() + roi.width() - 1); int y2 = std::min(grayForVisionGuide.rows - 1, roi.y() + roi.height() - 1); if (x2 <= x1 || y2 <= y1) { emit logMessage(QStringLiteral("[Seg] ROI 越界")); return false; } HObject roiRegion; GenRectangle1(&roiRegion, y1, x1, y2, x2); HObject finalDomain; double segCR = 0, segCC = 0; std::vector> contours; cv::Mat segmentMask; segOk = m_d->autoSegmentBrightMetal(gray, QRect(x1, y1, x2 - x1 + 1, y2 - y1 + 1), roiRegion, params.segDilation, finalDomain, areaRatio, segCR, segCC, &segmentMask); if (!segOk) { segOk = m_d->autoSegment(himg, roiRegion, params.segDilation, finalDomain, areaRatio, segCR, segCC); } if (segOk) { HObject xld; GenContourRegionXld(finalDomain, &xld, "border"); HTuple nC; CountObj(xld, &nC); for (int i = 1; i <= nC.I(); ++i) { HObject one; SelectObj(xld, &one, i); HTuple rr, cc; GetContourXld(one, &rr, &cc); std::vector pts; pts.reserve(rr.Length()); for (int k = 0; k < rr.Length(); ++k) pts.emplace_back(cvRound(cc[k].D()), cvRound(rr[k].D())); if (pts.size() >= 3) contours.push_back(std::move(pts)); } } if (!segmentMask.empty()) { segResultImg = Impl::makeSegmentationOverlay( grayForVisionGuide, QRect(x1, y1, x2 - x1 + 1, y2 - y1 + 1), segmentMask, areaRatio, segOk); } else { cv::cvtColor(grayForVisionGuide, segResultImg, cv::COLOR_GRAY2BGR); cv::rectangle(segResultImg, cv::Rect(x1, y1, x2-x1+1, y2-y1+1), cv::Scalar(0,165,255), 2); } if (segOk && !contours.empty()) { cv::Mat overlay = segResultImg.clone(); cv::fillPoly(overlay, contours, cv::Scalar(0,255,0)); cv::addWeighted(overlay, 0.30, segResultImg, 0.70, 0, segResultImg); cv::polylines(segResultImg, contours, true, cv::Scalar(0,255,0), 2, cv::LINE_AA); cv::Point C(cvRound(segCC), cvRound(segCR)); cv::drawMarker(segResultImg, C, cv::Scalar(0,0,255), cv::MARKER_CROSS, 22, 2); cv::circle(segResultImg, C, 4, cv::Scalar(0,0,255), -1); cv::putText(segResultImg, cv::format("Seg OK Area=%.1f%%", areaRatio*100), cv::Point(x1, std::max(30, y1-12)), cv::FONT_HERSHEY_SIMPLEX, 0.85, cv::Scalar(0,255,0), 2); } else { cv::putText(segResultImg, cv::format("Seg FAILED Area=%.1f%% (use ROI)", areaRatio*100), cv::Point(x1, std::max(30, y1-12)), cv::FONT_HERSHEY_SIMPLEX, 0.85, cv::Scalar(0,0,255), 2); } emit logMessage(QStringLiteral("[Step1] 分割: %1 面积占比=%2%") .arg(segOk ? QStringLiteral("成功") : QStringLiteral("失败")) .arg(areaRatio*100, 0, 'f', 2)); return true; } catch (HException& e) { emit logMessage(QStringLiteral("[VisionGuide] 分割异常: %1").arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())))); return false; } catch (cv::Exception& e) { emit logMessage(QStringLiteral("[OpenCV] 分割异常: %1").arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.what())))); return false; } } VisionPoseResult VisionGuideMatcher::stepPoseMatch(const cv::Mat& grayFull, const QRect& roi, CameraSide side) { return match(grayFull, roi, side); } DebugReport VisionGuideMatcher::stepDebugMatch(const cv::Mat& grayFull, const QRect& roi, CameraSide side) { DebugReport report; if (!isReady(side)) return report; try { cv::Mat grayFiltered = applyPreFilter(grayFull); if (!grayFiltered.isContinuous()) grayFiltered = grayFiltered.clone(); cv::Mat gray = grayFiltered.clone(); HObject himg = Impl::matToHImage(gray); HObject roiRegion; GenRectangle1(&roiRegion, roi.top(), roi.left(), roi.top()+roi.height()-1, roi.left()+roi.width()-1); HObject finalDomain; double areaRatio = 0, segCR = 0, segCC = 0; if (params.useAutoSeg) { if (!m_d->autoSegmentBrightMetal(gray, roi, roiRegion, params.segDilation, finalDomain, areaRatio, segCR, segCC)) { m_d->autoSegment(himg, roiRegion, params.segDilation, finalDomain, areaRatio, segCR, segCC); } } else { finalDomain = roiRegion; } HObject imgForMatch, imgMed3, imgMed5, imgMedGauss; ReduceDomain(himg, finalDomain, &imgForMatch); MedianImage(imgForMatch, &imgMed3, "circle", 3, "mirrored"); MedianImage(imgForMatch, &imgMed5, "circle", 5, "mirrored"); GaussFilter(imgMed3, &imgMedGauss, 3); struct PrepImg { QString name; HObject* img; }; PrepImg preps[] = { { QString::fromUtf8("原图 (无滤波)"), &imgForMatch }, { QString::fromUtf8("VisionGuide中值滤波(r=3)"), &imgMed3 }, { QString::fromUtf8("VisionGuide中值滤波(r=5)"), &imgMed5 }, { QString::fromUtf8("VisionGuide中值+高斯"), &imgMedGauss } }; double testMinScores[] = { params.minScore }; double bestScore = 0; int bestPrepIdx = -1; double bestMinScore = 0; HTuple bestPose, bestCovPose, bestScoreTuple; for (int prepIdx = 0; prepIdx < 4; ++prepIdx) { for (double testScore : testMinScores) { DebugMatchResult dmr; dmr.preprocessName = preps[prepIdx].name; dmr.minScore = testScore; QElapsedTimer t; t.start(); HTuple gn2, gv2; gn2 = HTuple("num_matches").TupleConcat("max_overlap") .TupleConcat("pose_refinement").TupleConcat("recompute_score"); gv2 = HTuple(1).TupleConcat(0.0) .TupleConcat(params.poseRefinement.isEmpty() ? "least_squares" : params.poseRefinement.toLatin1().constData()) .TupleConcat("true"); HTuple poseTest, covTest, scoreTest; FindShapeModel3d(*preps[prepIdx].img, m_d->shapeModelId[side], testScore, 0.85, 0, gn2, gv2, &poseTest, &covTest, &scoreTest); dmr.timeMs = t.elapsed(); if (scoreTest.Length() > 0) { dmr.found = true; dmr.score = scoreTest[0].D(); if (dmr.score > bestScore) { bestScore = dmr.score; bestPrepIdx = prepIdx; bestMinScore = testScore; bestPose = poseTest; bestCovPose = covTest; bestScoreTuple = scoreTest; } } report.results.push_back(dmr); } } report.bestPrepIdx = bestPrepIdx; report.bestMinScore = bestMinScore; report.bestScore = bestScore; if (bestPrepIdx >= 0 && bestPrepIdx < 4) report.bestPrepName = preps[bestPrepIdx].name; if (bestPrepIdx >= 0) m_d->parsePose(bestPose, bestCovPose, bestScoreTuple, 0, params.coordAxisLen_m, side, report.bestPose); } catch (HException& e) { emit logMessage(QString::fromUtf8("参数调试异常: %1").arg(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())))); } return report; } // ═══════════════════════════════════════════════════════════════ // 绘图 + 工具函数 // ═══════════════════════════════════════════════════════════════ bool VisionGuideMatcher::updatePoseProjection(VisionPoseResult& result, CameraSide side, bool updateContours) { if (!result.valid || side < CAM_LEFT || side > CAM_RIGHT || !m_d || !m_d->camLoaded[side] || (updateContours && !m_d->modelLoaded[side])) return false; try { HTuple pose; pose[0] = result.tx; pose[1] = result.ty; pose[2] = result.tz; pose[3] = result.rx; pose[4] = result.ry; pose[5] = result.rz; pose[6] = result.poseType; if (updateContours) { HObject projected; ProjectShapeModel3d(&projected, m_d->shapeModelId[side], m_d->camParam[side], pose, "true", HTuple(30).TupleRad()); result.projectedContours = Impl::extractContours(projected); } HTuple transform; PoseToHomMat3d(pose, &transform); HTuple row, col; Project3dPoint(result.tx, result.ty, result.tz, m_d->camParam[side], &row, &col); result.originRow = row.D(); result.originCol = col.D(); auto projectTip = [&](double dx, double dy, double dz, double& tipRow, double& tipCol) { HTuple x, y, z, r, c; AffineTransPoint3d(transform, dx, dy, dz, &x, &y, &z); Project3dPoint(x, y, z, m_d->camParam[side], &r, &c); tipRow = r.D(); tipCol = c.D(); }; const double axisLength = params.coordAxisLen_m; projectTip(axisLength, 0, 0, result.xTipRow, result.xTipCol); projectTip(0, axisLength, 0, result.yTipRow, result.yTipCol); projectTip(0, 0, axisLength, result.zTipRow, result.zTipCol); return true; } catch (HException& e) { emit logMessage(QString::fromUtf8("位姿投影更新失败: ") + sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA()))); return false; } } void VisionGuideMatcher::drawPoseResult(cv::Mat& img, const VisionPoseResult& r) { if (!r.valid) return; if (img.channels() == 1) cv::cvtColor(img, img, cv::COLOR_GRAY2BGR); for (const auto& poly : r.projectedContours) for (size_t i = 1; i < poly.size(); ++i) cv::line(img, cv::Point(cvRound(poly[i-1].x), cvRound(poly[i-1].y)), cv::Point(cvRound(poly[i].x), cvRound(poly[i].y)), cv::Scalar(0,255,0), 2, cv::LINE_AA); cv::Point O(cvRound(r.originCol), cvRound(r.originRow)); cv::Point X(cvRound(r.xTipCol), cvRound(r.xTipRow)); cv::Point Y(cvRound(r.yTipCol), cvRound(r.yTipRow)); cv::Point Z(cvRound(r.zTipCol), cvRound(r.zTipRow)); cv::arrowedLine(img, O, X, cv::Scalar(0,0,255), 4, cv::LINE_AA, 0, 0.15); cv::arrowedLine(img, O, Y, cv::Scalar(0,255,0), 4, cv::LINE_AA, 0, 0.15); cv::arrowedLine(img, O, Z, cv::Scalar(255,0,0), 4, cv::LINE_AA, 0, 0.15); cv::line(img, O+cv::Point(-15,0), O+cv::Point(15,0), cv::Scalar(0,255,255), 3, cv::LINE_AA); cv::line(img, O+cv::Point(0,-15), O+cv::Point(0,15), cv::Scalar(0,255,255), 3, cv::LINE_AA); cv::putText(img, "X", X+cv::Point(8,-8), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255), 2, cv::LINE_AA); cv::putText(img, "Y", Y+cv::Point(8,-8), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,255,0), 2, cv::LINE_AA); cv::putText(img, "Z", Z+cv::Point(8,-8), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(255,0,0), 2, cv::LINE_AA); int y0 = 30; auto putLine = [&](const std::string& text, cv::Scalar color) { cv::putText(img, text, cv::Point(15, y0), cv::FONT_HERSHEY_SIMPLEX, 0.55, color, 1, cv::LINE_AA); y0 += 22; }; putLine(cv::format("Score: %.4f Time: %.0fms", r.score, r.matchTimeMs), cv::Scalar(255,255,255)); putLine(cv::format("X=%.2fmm Y=%.2fmm Z=%.2fmm", r.tx*1000, r.ty*1000, r.tz*1000), cv::Scalar(255,255,255)); putLine(cv::format("Rx=%.2f Ry=%.2f Rz=%.2f", r.rx, r.ry, r.rz), cv::Scalar(255,255,255)); } void VisionGuideMatcher::drawSegResult(cv::Mat& img, const QRect& roi, bool segOk, double areaRatio) { if (img.channels() == 1) cv::cvtColor(img, img, cv::COLOR_GRAY2BGR); cv::rectangle(img, cv::Rect(roi.x(), roi.y(), roi.width(), roi.height()), cv::Scalar(0,165,255), 2); if (segOk) cv::putText(img, cv::format("Seg OK Area=%.1f%%", areaRatio*100), cv::Point(roi.x(), roi.y()-10), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,255,0), 2); else cv::putText(img, "Seg FAILED", cv::Point(roi.x(), roi.y()-10), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255), 2); } QImage VisionGuideMatcher::matToQImage(const cv::Mat& mat) { if (mat.empty()) return QImage(); if (mat.type() == CV_8UC1) return QImage(mat.data, mat.cols, mat.rows, (int)mat.step, QImage::Format_Grayscale8).copy(); if (mat.type() == CV_8UC3) { cv::Mat rgb; cv::cvtColor(mat, rgb, cv::COLOR_BGR2RGB); return QImage(rgb.data, rgb.cols, rgb.rows, (int)rgb.step, QImage::Format_RGB888).copy(); } return QImage(); } cv::Mat VisionGuideMatcher::qImageToMat(const QImage& img) { if (img.isNull()) return cv::Mat(); QImage rgb = img.convertToFormat(QImage::Format_RGB888); cv::Mat mat(rgb.height(), rgb.width(), CV_8UC3, (void*)rgb.constBits(), rgb.bytesPerLine()); return mat.clone(); }