update 优化pin脚投影矩阵计算

AlgorithmModule/src/ImgCheckAnalysisy.cpp

@@ -59,6 +59,57 @@ static cv::Point2f ApplyAffine2D(const cv::Mat &M, const cv::Point2f &pt)
 					   static_cast<float>(M.at<double>(1, 0) * pt.x + M.at<double>(1, 1) * pt.y + M.at<double>(1, 2)));
 }
 
+// 最小二乘拟合相似变换(旋转+等比缩放+平移): src(模板点) -> dst(当前点), 返回 CV_64F 2x3
+// 用全部匹配点做闭式解(不剔外点), 使投影后的残差均值为 0 —— 即模板 Pin 整体居中于当前 Pin;
+// 注意: 这一步很关键, 只做"硬贴某一个 pin 对"的平移会让整排留下几像素的系统偏移
+static cv::Mat FitSimilarityLS(const std::vector<cv::Point2f> &src, const std::vector<cv::Point2f> &dst)
+{
+	const size_t n = src.size();
+	if (n < 2 || dst.size() != n)
+	{
+		return cv::Mat();
+	}
+
+	double mxs = 0.0, mys = 0.0, mxd = 0.0, myd = 0.0;
+	for (size_t i = 0; i < n; i++)
+	{
+		mxs += src[i].x;
+		mys += src[i].y;
+		mxd += dst[i].x;
+		myd += dst[i].y;
+	}
+	mxs /= n;
+	mys /= n;
+	mxd /= n;
+	myd /= n;
+
+	double sdp = 0.0; // Σ p·q
+	double scp = 0.0; // Σ p×q
+	double den = 0.0; // Σ |p|^2
+	for (size_t i = 0; i < n; i++)
+	{
+		const double px = src[i].x - mxs;
+		const double py = src[i].y - mys;
+		const double qx = dst[i].x - mxd;
+		const double qy = dst[i].y - myd;
+		sdp += px * qx + py * qy;
+		scp += px * qy - py * qx;
+		den += px * px + py * py;
+	}
+	if (den <= 1e-12)
+	{
+		return cv::Mat();
+	}
+
+	const double scale = std::sqrt(sdp * sdp + scp * scp) / den;
+	const double theta = std::atan2(scp, sdp);
+	const double a = scale * std::cos(theta);
+	const double b = -scale * std::sin(theta);
+	const double tx = mxd - (a * mxs + b * mys);
+	const double ty = myd - (-b * mxs + a * mys);
+	return (cv::Mat_<double>(2, 3) << a, b, tx, -b, a, ty);
+}
+
 // 把 Pin 中心按"排"分组(最多两排), 用于内定位法线无法分排时的兜底:
 //   用 minAreaRect 求 Pin 群主方向, 沿短边(法线)投影, 在最大间隙处切分;
 //   间隙过小(单排)或切分后某组不足 2 个 Pin 时按一排处理。
@@ -2658,7 +2709,7 @@ int ImgCheckAnalysisy::ComputePinProjectionMats()
 	};
 
 	// 用模板 Pin 组 rowIdx 与当前 Pin 候选组 curIdxList 拟合一个相似变换(模板图坐标 -> detImg 坐标)
-	auto fitOneRow = [&](const std::vector<int> &rowIdx, const std::vector<int> &curIdxList, PinRowFit &fit)
+	auto fitOneRow = [&](const std::vector<int> &rowIdx, const std::vector<int> &curIdxList, int rowNo, PinRowFit &fit)
 	{
 		const int nRow = static_cast<int>(rowIdx.size());
 		const int nCurSub = static_cast<int>(curIdxList.size());
@@ -2789,8 +2840,22 @@ int ImgCheckAnalysisy::ComputePinProjectionMats()
 			return static_cast<int>(matches.size());
 		};
 
+		// 匹配残差(平方和): 用于"重合数相同"时挑出真正居中的解
+		auto residualOf = [&](const cv::Mat &M, const std::vector<std::pair<int, int>> &matchList) -> double
+		{
+			double sum = 0.0;
+			for (size_t k = 0; k < matchList.size(); k++)
+			{
+				const cv::Point2f p = ApplyAffine2D(M, tplCenters[rowIdx[matchList[k].first]]);
+				const cv::Point2f d = curCenters[matchList[k].second] - p;
+				sum += static_cast<double>(d.x) * d.x + static_cast<double>(d.y) * d.y;
+			}
+			return sum;
+		};
+
 		std::vector<std::pair<int, int>> matches;
 		int bestOverlap = matchPins(MCur, matches);
+		double bestErr = residualOf(MCur, matches);
 		cv::Mat bestM = MCur.clone();
 		for (int iter = 0; iter < 5 && bestOverlap < nRow; iter++)
 		{
@@ -2814,19 +2879,67 @@ int ImgCheckAnalysisy::ComputePinProjectionMats()
 				break;
 			}
 			const int cnt = matchPins(MNew, matches);
-			if (cnt > bestOverlap)
+			const double err = residualOf(MNew, matches);
+			// 重合数更多, 或重合数相同但残差更小 -> 采用(避免停留在"只硬贴某一个 pin 对"的投票解上)
+			if (cnt > bestOverlap || (cnt == bestOverlap && err < bestErr))
 			{
 				bestOverlap = cnt;
+				bestErr = err;
 				bestM = MNew.clone();
 			}
 			MCur = MNew;
 		}
 
+		// 最终用全部重合对再做一次最小二乘(闭式解)相似变换:
+		// 让模板投影相对当前 Pin 的残差均值为 0(整体居中), 消除"整排几像素的系统偏移"
+		if (bestOverlap >= 2)
+		{
+			matchPins(bestM, matches);
+			if (matches.size() >= 2)
+			{
+				std::vector<cv::Point2f> srcPts;
+				std::vector<cv::Point2f> dstPts;
+				srcPts.reserve(matches.size());
+				dstPts.reserve(matches.size());
+				for (size_t k = 0; k < matches.size(); k++)
+				{
+					srcPts.push_back(tplCenters[rowIdx[matches[k].first]]);
+					dstPts.push_back(curCenters[matches[k].second]);
+				}
+				cv::Mat MRefine = FitSimilarityLS(srcPts, dstPts);
+				// 重合数不下降才采用(避免把错配点带进最小二乘)
+				if (bReasonableMat(MRefine) && matchPins(MRefine, matches) >= bestOverlap)
+				{
+					bestM = MRefine.clone();
+					bestOverlap = static_cast<int>(matches.size());
+				}
+			}
+		}
+
 		if (bestOverlap >= 2 && bReasonableMat(bestM))
 		{
 			fit.M = bestM;
 			fit.nMatch = bestOverlap;
 			fit.bFromPins = true;
+
+			// 诊断: 模板投影相对当前 Pin 的残差(均值应接近 0, rms 反映重合程度)
+			matchPins(bestM, matches);
+			double sumX = 0.0, sumY = 0.0, sumSq = 0.0;
+			for (size_t k = 0; k < matches.size(); k++)
+			{
+				const cv::Point2f p = ApplyAffine2D(bestM, tplCenters[rowIdx[matches[k].first]]);
+				const cv::Point2f d = curCenters[matches[k].second] - p;
+				sumX += d.x;
+				sumY += d.y;
+				sumSq += static_cast<double>(d.x) * d.x + static_cast<double>(d.y) * d.y;
+			}
+			const int nM = static_cast<int>(matches.size());
+			if (nM > 0)
+			{
+				m_pdetlog->AddCheckstr(PrintLevel_1, DET_LOG_LEVEL_3, "Pin Matrix",
+									   "row %d residual: mean(%.2f %.2f) px  rms %.2f px  n %d",
+									   rowNo, sumX / nM, sumY / nM, std::sqrt(sumSq / nM), nM);
+			}
 		}
 	};
 
@@ -2839,7 +2952,7 @@ int ImgCheckAnalysisy::ComputePinProjectionMats()
 		{
 			for (int b = 0; b < 2; b++)
 			{
-				fitOneRow(rows[a], curRows[b], fits[a][b]);
+				fitOneRow(rows[a], curRows[b], a, fits[a][b]);
 			}
 		}
 		const int scoreDirect = fits[0][0].nMatch + fits[1][1].nMatch;
@@ -2862,7 +2975,7 @@ int ImgCheckAnalysisy::ComputePinProjectionMats()
 		// 无法两侧都分排: 每排与全部当前 Pin 匹配
 		for (size_t r = 0; r < rows.size(); r++)
 		{
-			fitOneRow(rows[r], allCurIdx, rowFits[r]);
+			fitOneRow(rows[r], allCurIdx, static_cast<int>(r), rowFits[r]);
 		}
 	}