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

1349 lines
60 KiB
C++

// ════════════════════════════════════════════════════════════════════════════
// VisionGuideMatcher.cpp ─ VisionGuide 3D Shape-Based Matching 封装实现 (v4)
//
// [v3] 左右相机分别加载标定参数和轮廓库(CameraSide)
// [v4] 新增 matchMultiple() — 全图多目标匹配, 无ROI, 无分割
// ════════════════════════════════════════════════════════════════════════════
#include "UserVisibleLog.h"
#include "VisionGuideMatcher.h"
#define NOMINMAX
#include <VisionGuideApi.h>
#include <QElapsedTimer>
#include <QDebug>
#include <QFile>
#include <QCryptographicHash>
#include <algorithm>
#include <cmath>
#include <limits>
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<std::vector<cv::Point2d>> extractContours(const HObject& xldContours)
{
std::vector<std::vector<cv::Point2d>> 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<cv::Point2d> 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<uchar>(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<double> 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<int>(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<std::vector<cv::Point>> 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<uchar>(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<cv::CLAHE> 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<int>(i, cv::CC_STAT_AREA);
if (area < std::max(8, minSeedPixels / 6))
continue;
const double dx = (centroids.at<double>(i, 0) - roiCx) / std::max(1, rc.width);
const double dy = (centroids.at<double>(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<cv::Point2d> pts;
pts.reserve(cv::countNonZero(cleanSeed));
for (int y = 0; y < cleanSeed.rows; ++y) {
const uchar* row = cleanSeed.ptr<uchar>(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<double>(i, 0) = pts[i].x;
data.at<double>(i, 1) = pts[i].y;
}
cv::PCA pca(data, cv::Mat(), cv::PCA::DATA_AS_ROW);
cv::Point2d center(pca.mean.at<double>(0, 0), pca.mean.at<double>(0, 1));
cv::Point2d axis(pca.eigenvectors.at<double>(0, 0), pca.eigenvectors.at<double>(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<double> longVals;
std::vector<double> 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<cv::Point> 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, &region, 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<Impl>()) {}
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<double>::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<double>::infinity();
double minRow = std::numeric_limits<double>::infinity();
double maxCol = -std::numeric_limits<double>::infinity();
double maxRow = -std::numeric_limits<double>::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<VisionPoseResult> VisionGuideMatcher::matchMultiple(
const cv::Mat& grayFull, int numMatches, CameraSide side, const QRect& searchRoi)
{
std::vector<VisionPoseResult> 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<VisionPoseResult> VisionGuideMatcher::matchMultipleInMask(
const cv::Mat& grayFull, const cv::Mat& domainMask, int numMatches, CameraSide side)
{
std::vector<VisionPoseResult> 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<std::vector<cv::Point>> 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<cv::Point> 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();
}