1349 lines
60 KiB
C++
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, ®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<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();
|
|
}
|