using HalconDotNet;
|
using LB_VisionProcesses.Alogrithms.Halcon;
|
using System;
|
using System.Collections.Generic;
|
using System.Linq;
|
using System.Text;
|
using System.Threading.Tasks;
|
using OpenCvSharp;
|
using Size = OpenCvSharp.Size;
|
using Point = OpenCvSharp.Point;
|
using static LB_VisionProcesses.Alogrithms.OpenCvSharp.FindLineTool;
|
using System.Diagnostics;
|
using LB_VisionControl;
|
|
namespace LB_VisionProcesses.Alogrithms.OpenCvSharp
|
{
|
public enum Transition { Positive, Negative, Ignore }
|
|
public enum Selector { First, Last, Best }
|
[Process("OpenCvSharp_找线工具", Category = "OpenCvSharp工具", Description = "创建OpenCvSharp_找线工具")]
|
public class FindLineTool : TAlgorithm
|
{
|
public FindLineTool()
|
{
|
strProcessClass = "LB_VisionProcesses.Alogrithms.OpenCvSharp.FindLineTool";
|
strProcessName = "OpenCvSharp_找线工具";
|
|
Params.Inputs.Add("卡尺数量", 6);
|
Params.Inputs.Add("卡尺长度", 30);
|
Params.Inputs.Add("卡尺宽度", 10);
|
Params.Inputs.Add("过滤一半像素", 2);
|
Params.Inputs.Add("对比度阈值", 5);
|
Params.Inputs.Add("极性", "Ignore");
|
Params.Inputs.Add("边缘位置", "Best");
|
Params.Inputs.Add("忽略点数", 0);
|
|
Params.Outputs.Add("X", new List<double>());
|
Params.Outputs.Add("Y", new List<double>());
|
Params.Outputs.Add("CenterX", 0.0);
|
Params.Outputs.Add("CenterY", 0.0);
|
Params.Outputs.Add("Phi", 0.0);
|
Params.Outputs.Add("Angle", 0.0);
|
Params.Outputs.Add("Count", 0);
|
Params.Outputs.Add("Segment", new HSegment());
|
|
Params.ROI = new HSegment(0, 0, 250, 250);
|
}
|
|
/// <summary>
|
/// 算子逻辑
|
/// </summary>
|
public override void TAlgorithmMain()
|
{
|
#region 初始化变量
|
HObject ho_Regions, ho_LineXld;
|
HOperatorSet.GenEmptyObj(out ho_Regions);
|
HOperatorSet.GenEmptyObj(out ho_LineXld);
|
#endregion
|
|
try
|
{
|
if (InputImage == null)
|
{
|
Msg = "输入图片为空";
|
Result = false;
|
return;
|
}
|
|
#region 裁剪区域
|
object DomainImage = null;
|
if (!ReduceDomainImage(InputImage, ref DomainImage))
|
{
|
Msg = "裁剪区域失败";
|
Result = false;
|
return;
|
}
|
|
Mat hoDomainImage = DomainImage as Mat;
|
//判断是否为灰度图
|
try
|
{
|
if (hoDomainImage.Channels() != 1)
|
Cv2.CvtColor(hoDomainImage, hoDomainImage, ColorConversionCodes.RGB2GRAY);
|
|
//转换后再次检查是否为灰度图
|
if (hoDomainImage.Channels() != 1)
|
{
|
Msg = "输入图片不为灰度图";
|
Result = false;
|
return;
|
}
|
}
|
catch
|
{
|
Msg = "输入图片不为灰度图且转换失败";
|
Result = false;
|
return;
|
}
|
#endregion
|
|
|
#region 算子逻辑
|
Record = new ObjectRecord();
|
|
int hv_Elements = Convert.ToInt16(Params.Inputs["卡尺数量"]);
|
double hv_DetectHeight = Convert.ToDouble(Params.Inputs["卡尺长度"]);
|
double hv_DetectWidth = Convert.ToDouble(Params.Inputs["卡尺宽度"]);
|
int hv_Sigma = Convert.ToInt16(Params.Inputs["过滤一半像素"]);
|
int hv_Threshold = Convert.ToInt16(Params.Inputs["对比度阈值"]);
|
int hv_IgnoreNum = Convert.ToInt16(Params.Inputs["忽略点数"]);
|
|
Enum.TryParse(Params.Inputs["极性"]?.ToString(), out Transition transition);
|
string hv_Transition = "ignore";
|
switch (transition)
|
{
|
case Transition.Positive:
|
hv_Transition = "positive";
|
break;
|
case Transition.Negative:
|
hv_Transition = "negative";
|
break;
|
case Transition.Ignore:
|
default:
|
hv_Transition = "ignore";
|
break;
|
}
|
double hv_Row1 = Convert.ToDouble(((HSegment)Params.ROI).BeginRow + Params.Fixture.Row);
|
double hv_Column1 = Convert.ToDouble(((HSegment)Params.ROI).BeginColumn + Params.Fixture.Column);
|
double hv_Row2 = Convert.ToDouble(((HSegment)Params.ROI).EndRow + Params.Fixture.Row);
|
double hv_Column2 = Convert.ToDouble(((HSegment)Params.ROI).EndColumn + Params.Fixture.Column);
|
|
// 执行检测
|
var lines = DetectEdgePoints(hoDomainImage, new Point(hv_Column1, hv_Row1), new Point(hv_Column2, hv_Row2));
|
|
if (lines.Count <= 0)
|
{
|
Msg = "找线失败";
|
Result = false;
|
return;
|
}
|
#endregion
|
|
#region 结果处理
|
List<double> X = new List<double>();
|
List<double> Y = new List<double>();
|
double CenterX = 0;
|
double CenterY = 0;
|
double Phi = 0;
|
HSegment HSegment = new HSegment();
|
|
var contourArray = lines.ToArray();
|
int count = contourArray.Length;
|
|
double[] Xpoints = new double[count];
|
double[] Ypoints = new double[count];
|
|
Parallel.For(0, count, j =>
|
{
|
var point = contourArray[j];
|
Xpoints[j] = Convert.ToDouble(point.X);
|
Ypoints[j] = Convert.ToDouble(point.Y);
|
});
|
|
X.AddRange(Xpoints);
|
Y.AddRange(Ypoints);
|
|
var resultSegment = FitLineToPoints(lines);
|
|
pts_to_best_line(out ho_LineXld, new HTuple(Ypoints), new HTuple(Xpoints), hv_IgnoreNum
|
, out HTuple hv_Row11, out HTuple hv_Column11, out HTuple hv_Row21, out HTuple hv_Column21);
|
((ObjectRecord)Record).AddXld(ho_LineXld);
|
CenterX = (hv_Column11.D + hv_Column21.D) / 2;
|
CenterY = (hv_Row11.D + hv_Row21.D) / 2;
|
//计算直线与x轴的夹角，逆时针方向为正向
|
HOperatorSet.AngleLx(hv_Row11, hv_Column11, hv_Row21, hv_Column21, out HTuple hv_ATan);
|
Phi = hv_ATan;
|
|
if (X.Count >= 2)
|
HSegment = new HSegment(hv_Column11.D, hv_Row11.D, hv_Column21.D, hv_Row21.D);
|
|
//HOperatorSet.GenRegionLine(out ho_LineXld, resultSegment.StartPoint.Y, resultSegment.StartPoint.X
|
// , resultSegment.EndPoint.Y, resultSegment.EndPoint.X);
|
//((ObjectRecord)Record).AddXld(ho_LineXld);
|
|
//CenterX = (resultSegment.StartPoint.X + resultSegment.EndPoint.X) / 2;
|
//CenterY = (resultSegment.StartPoint.Y + resultSegment.EndPoint.Y) / 2;
|
|
//Phi = resultSegment.Angle / 180 * Math.PI;
|
|
//if (X.Count >= 2)
|
// HSegment = new HSegment(resultSegment.StartPoint.X, resultSegment.StartPoint.Y
|
// , resultSegment.EndPoint.X, resultSegment.EndPoint.Y);
|
|
Params.Outputs["X"] = X;
|
Params.Outputs["Y"] = Y;
|
Params.Outputs["CenterX"] = CenterX;
|
Params.Outputs["CenterY"] = CenterY;
|
Params.Outputs["Phi"] = Phi;
|
Params.Outputs["Angle"] = Phi * 180.0 / Math.PI;
|
Params.Outputs["Count"] = X.Count;
|
Params.Outputs["Segment"] = HSegment;
|
#endregion
|
|
#region 生成OutputImage给后续处理
|
try
|
{
|
OutputImage = hoDomainImage;
|
}
|
catch (Exception ex)
|
{
|
Msg = "生成OutputImage失败,原因是:" + ex.ToString();
|
Result = false;
|
return;
|
}
|
#endregion
|
|
if (Msg == "运行超时")
|
{
|
Result = false;
|
return;
|
}
|
|
Msg = "运行成功";
|
Result = true;
|
return;
|
}
|
catch (Exception ex)
|
{
|
Msg = "运行失败,原因是:" + ex.ToString().TrimEnd();
|
OutputImage = null;
|
Result = false;
|
return;
|
}
|
finally
|
{
|
if (!Result)
|
{
|
Params.Outputs.Add("X", new List<double>());
|
Params.Outputs.Add("Y", new List<double>());
|
Params.Outputs.Add("CenterX", 0.0);
|
Params.Outputs.Add("CenterY", 0.0);
|
Params.Outputs.Add("Phi", 0.0);
|
Params.Outputs.Add("Angle", 0.0);
|
Params.Outputs.Add("Count", 0);
|
Params.Outputs.Add("Segment", new HSegment());
|
}
|
|
bCompleted = true;
|
#region 内存释放
|
ho_Regions.Dispose();
|
#endregion
|
}
|
}
|
|
/// <summary>
|
/// 直线段数据
|
/// </summary>
|
public class LineSegment
|
{
|
public Point2f StartPoint { get; set; }
|
public Point2f EndPoint { get; set; }
|
|
public List<Point2f> EdgePoints { get; set; }
|
|
public float Vx { get; set; } // 方向向量X
|
public float Vy { get; set; } // 方向向量Y
|
public float Score { get; set; } // 拟合分数 0-100
|
public int RegionIndex { get; set; } // 区域索引
|
|
public double Length => Math.Sqrt(
|
Math.Pow(EndPoint.X - StartPoint.X, 2) +
|
Math.Pow(EndPoint.Y - StartPoint.Y, 2));
|
|
public double Angle => Math.Atan2(Vy, Vx) * 180 / Math.PI;
|
|
public LineSegment() { }
|
|
public LineSegment(Point2f start, Point2f end, float vx, float vy, List<Point2f> edgePoints)
|
{
|
StartPoint = start;
|
EndPoint = end;
|
Vx = vx;
|
Vy = vy;
|
EdgePoints = edgePoints;
|
}
|
}
|
|
/// <summary>
|
/// 过滤无效点
|
/// </summary>
|
private List<Point2f> FilterValidPoints(List<Point2f> points)
|
{
|
if (points == null) return new List<Point2f>();
|
|
return points.Where(p =>
|
!float.IsNaN(p.X) && !float.IsNaN(p.Y) &&
|
!float.IsInfinity(p.X) && !float.IsInfinity(p.Y) &&
|
p.X >= 0 && p.Y >= 0).ToList();
|
}
|
|
/// <summary>
|
/// 检测边缘点（使用卡尺检测方式）
|
/// </summary>
|
private List<Point2f> DetectEdgePoints(Mat regionImage, Point startPoint, Point endPoint)
|
{
|
var edgePoints = new List<Point2f>();
|
|
try
|
{
|
// 转换为灰度
|
var gray = new Mat();
|
if (regionImage.Channels() != 1)
|
Cv2.CvtColor(regionImage, gray, ColorConversionCodes.RGB2GRAY);
|
else
|
gray = regionImage.Clone();
|
|
// 获取参数
|
int caliperCount = Params.Inputs.ContainsKey("卡尺数量") ?
|
Convert.ToInt32(Params.Inputs["卡尺数量"]) : 6;
|
int caliperLength = Params.Inputs.ContainsKey("卡尺长度") ?
|
Convert.ToInt32(Params.Inputs["卡尺长度"]) : 30;
|
int caliperWidth = Params.Inputs.ContainsKey("卡尺宽度") ?
|
Convert.ToInt32(Params.Inputs["卡尺宽度"]) : 10;
|
int filterHalfPixels = Params.Inputs.ContainsKey("过滤一半像素") ?
|
Convert.ToInt32(Params.Inputs["过滤一半像素"]) : 2;
|
int contrastThreshold = Params.Inputs.ContainsKey("对比度阈值") ?
|
Convert.ToInt32(Params.Inputs["对比度阈值"]) : 5;
|
string polarity = Params.Inputs.ContainsKey("极性") ?
|
Params.Inputs["极性"]?.ToString() : "Ignore";
|
string edgePosition = Params.Inputs.ContainsKey("边缘位置") ?
|
Params.Inputs["边缘位置"]?.ToString() : "Best";
|
|
// 使用卡尺方式检测边缘点，传入起始点和终点
|
edgePoints = DetectEdgesAlongLineWithCaliper(
|
gray,
|
startPoint,
|
endPoint,
|
caliperCount,
|
caliperLength,
|
caliperWidth,
|
contrastThreshold,
|
polarity,
|
edgePosition);
|
|
// 过滤掉无效的点
|
edgePoints = FilterValidPoints(edgePoints);
|
}
|
catch (Exception ex)
|
{
|
Debug.WriteLine($"边缘点检测错误: {ex.Message}");
|
}
|
|
return edgePoints ?? new List<Point2f>();
|
}
|
|
/// <summary>
|
/// 沿指定线段使用多个卡尺检测边缘点
|
/// </summary>
|
private List<Point2f> DetectEdgesAlongLineWithCaliper(
|
Mat grayImage,
|
Point startPoint,
|
Point endPoint,
|
int caliperCount,
|
int caliperLength,
|
int caliperWidth,
|
int contrastThreshold,
|
string polarity,
|
string edgePosition)
|
{
|
var edgePoints = new List<Point2f>();
|
|
// 计算主线段方向
|
double dx = endPoint.X - startPoint.X;
|
double dy = endPoint.Y - startPoint.Y;
|
double mainLength = Math.Sqrt(dx * dx + dy * dy);
|
|
if (mainLength < 1) return edgePoints;
|
|
// 主线段单位方向向量
|
double mainUx = dx / mainLength;
|
double mainUy = dy / mainLength;
|
|
// 主线段垂直方向向量（卡尺方向）
|
double caliperUx = -mainUy;
|
double caliperUy = mainUx;
|
|
// 在主线段上均匀分布卡尺
|
for (int i = 0; i < caliperCount; i++)
|
{
|
double t = (i + 1.0) / (caliperCount + 1); // 在0-1之间均匀分布
|
double centerX = startPoint.X + t * mainLength * mainUx;
|
double centerY = startPoint.Y + t * mainLength * mainUy;
|
|
// 计算卡尺的起点和终点（垂直于主线段方向）
|
Point caliperStart = new Point(
|
(int)Math.Round(centerX - caliperLength / 2.0 * caliperUx),
|
(int)Math.Round(centerY - caliperLength / 2.0 * caliperUy));
|
|
Point caliperEnd = new Point(
|
(int)Math.Round(centerX + caliperLength / 2.0 * caliperUx),
|
(int)Math.Round(centerY + caliperLength / 2.0 * caliperUy));
|
|
// 检测单个卡尺上的边缘点
|
var edges = DetectEdgesAlongLine(
|
grayImage,
|
caliperStart.X, caliperStart.Y,
|
caliperEnd.X, caliperEnd.Y,
|
caliperWidth,
|
contrastThreshold,
|
polarity,
|
edgePosition);
|
edgePoints.AddRange(edges);
|
}
|
|
return edgePoints;
|
}
|
|
/// <summary>
|
/// 沿直线检测边缘点
|
/// </summary>
|
private List<Point2f> DetectEdgesAlongLine(
|
Mat grayImage,
|
int startX, int startY,
|
int endX, int endY,
|
int lineWidth,
|
int contrastThreshold,
|
string polarity, string edgePosition)
|
{
|
var edgePoints = new List<Point2f>();
|
|
// 计算直线方向
|
double dx = endX - startX;
|
double dy = endY - startY;
|
double length = Math.Sqrt(dx * dx + dy * dy);
|
|
if (length == 0) return edgePoints;
|
|
// 单位方向向量
|
double ux = dx / length;
|
double uy = dy / length;
|
|
// 垂直方向向量
|
double vx = -uy;
|
double vy = ux;
|
|
// 沿线采样
|
for (double t = 0; t <= length; t += 1.0)
|
{
|
double centerX = startX + t * ux;
|
double centerY = startY + t * uy;
|
|
// 在垂直方向上进行采样
|
List<double> profile = new List<double>();
|
List<Point> profilePoints = new List<Point>();
|
|
for (int w = -lineWidth / 2; w <= lineWidth / 2; w++)
|
{
|
int sampleX = (int)Math.Round(centerX + w * vx);
|
int sampleY = (int)Math.Round(centerY + w * vy);
|
|
if (sampleX >= 0 && sampleX < grayImage.Width &&
|
sampleY >= 0 && sampleY < grayImage.Height)
|
{
|
byte pixelValue = grayImage.At<byte>(sampleY, sampleX);
|
profile.Add(pixelValue);
|
profilePoints.Add(new Point(sampleX, sampleY));
|
}
|
}
|
|
if (profile.Count > 0)
|
{
|
// 分析灰度剖面，检测边缘
|
if (Enum.TryParse(polarity, out Transition transition))
|
{
|
var edgeCandidates = FindEdgesInProfile(profile, profilePoints, contrastThreshold, transition, edgePosition);
|
edgePoints.AddRange(edgeCandidates);
|
}
|
else
|
{
|
var edgeCandidates = FindEdgesInProfile(profile, profilePoints, contrastThreshold, Transition.Ignore, edgePosition);
|
edgePoints.AddRange(edgeCandidates);
|
}
|
}
|
}
|
|
return edgePoints;
|
}
|
|
/// <summary>
|
/// 在灰度剖面中查找边缘点
|
/// </summary>
|
private List<Point2f> FindEdgesInProfile(
|
List<double> profile,
|
List<Point> profilePoints,
|
int contrastThreshold,
|
Transition polarity, string edgePosition)
|
{
|
var edges = new List<Point2f>();
|
|
if (profile.Count < 3) return edges;
|
|
// 计算梯度
|
List<double> gradients = new List<double>();
|
for (int i = 1; i < profile.Count - 1; i++)
|
{
|
double gradient = (profile[i + 1] - profile[i - 1]) / 2.0;
|
gradients.Add(gradient);
|
}
|
|
// 根据极性过滤边缘
|
var edgeCandidates = new List<(int index, double strength)>();
|
|
for (int i = 0; i < gradients.Count; i++)
|
{
|
double gradient = gradients[i];
|
bool isEdge = false;
|
|
if (polarity == Transition.Positive && gradient > contrastThreshold)
|
{
|
isEdge = true; // 从暗到亮的边缘
|
}
|
else if (polarity == Transition.Negative && gradient < -contrastThreshold)
|
{
|
isEdge = true; // 从亮到暗的边缘
|
}
|
else if (polarity == Transition.Ignore && Math.Abs(gradient) > contrastThreshold)
|
{
|
isEdge = true; // 忽略极性，只关注对比度
|
}
|
|
if (isEdge)
|
{
|
edgeCandidates.Add((i + 1, Math.Abs(gradient)));
|
}
|
}
|
|
// 根据边缘位置参数选择边缘点
|
if (edgeCandidates.Count > 0)
|
{
|
if (Enum.TryParse(edgePosition, out Selector selector))
|
{
|
var selectedEdge = SelectEdgeByPosition(edgeCandidates, profile, selector);
|
if (selectedEdge.index >= 0)
|
{
|
Point2f edgePoint = RefineEdgePosition(profile, profilePoints, selectedEdge.index);
|
edges.Add(edgePoint);
|
}
|
}
|
else
|
{
|
var selectedEdge = SelectEdgeByPosition(edgeCandidates, profile, Selector.Best);
|
if (selectedEdge.index >= 0)
|
{
|
Point2f edgePoint = RefineEdgePosition(profile, profilePoints, selectedEdge.index);
|
edges.Add(edgePoint);
|
}
|
}
|
}
|
|
return edges;
|
}
|
|
/// <summary>
|
/// 根据边缘位置参数选择边缘点
|
/// </summary>
|
private (int index, double strength) SelectEdgeByPosition(
|
List<(int index, double strength)> edgeCandidates,
|
List<double> profile,
|
Selector edgePosition)
|
{
|
if (edgeCandidates.Count == 0) return (-1, 0);
|
|
switch (edgePosition)
|
{
|
case Selector.Best: // 最强边缘
|
return edgeCandidates.OrderByDescending(e => e.strength).First();
|
|
case Selector.First: // 第一个边缘
|
return edgeCandidates.OrderBy(e => e.index).First();
|
|
case Selector.Last: // 最后一个边缘
|
return edgeCandidates.OrderByDescending(e => e.index).First();
|
|
default: // 默认使用最强边缘
|
return edgeCandidates.OrderByDescending(e => e.strength).First();
|
}
|
}
|
|
/// <summary>
|
/// 亚像素精度边缘定位
|
/// </summary>
|
private Point2f RefineEdgePosition(List<double> profile, List<Point> profilePoints, int edgeIndex)
|
{
|
if (edgeIndex <= 0 || edgeIndex >= profile.Count - 1)
|
return new Point2f(profilePoints[edgeIndex].X, profilePoints[edgeIndex].Y);
|
|
// 简单的二次插值
|
double y0 = profile[edgeIndex - 1];
|
double y1 = profile[edgeIndex];
|
double y2 = profile[edgeIndex + 1];
|
|
// 计算亚像素偏移
|
double offset = (y0 - y2) / (2 * (y0 - 2 * y1 + y2));
|
|
// 线性插值计算亚像素位置
|
Point p0 = profilePoints[edgeIndex - 1];
|
Point p1 = profilePoints[edgeIndex];
|
Point p2 = profilePoints[edgeIndex + 1];
|
|
double subPixelX = p1.X + offset * (p2.X - p0.X) / 2.0;
|
double subPixelY = p1.Y + offset * (p2.Y - p0.Y) / 2.0;
|
|
return new Point2f((float)subPixelX, (float)subPixelY);
|
}
|
|
|
/// <summary>
|
/// 拟合直线
|
/// </summary>
|
private LineSegment FitLineToPoints(List<Point2f> points, float LineExtension = 5.0f)
|
{
|
if (points.Count < 2) return null;
|
|
try
|
{
|
// 使用OpenCV的FitLine
|
var lineOutput = new Mat();
|
using (var pointsArray = InputArray.Create(points))
|
{
|
Cv2.FitLine(pointsArray, lineOutput, DistanceTypes.L2, 0, 0.01, 0.01);
|
}
|
|
// 解析拟合结果: [vx, vy, x0, y0]
|
float vx = lineOutput.At<float>(0);
|
float vy = lineOutput.At<float>(1);
|
float x0 = lineOutput.At<float>(2);
|
float y0 = lineOutput.At<float>(3);
|
|
// 检查拟合结果是否有效
|
if (float.IsNaN(vx) || float.IsNaN(vy) || float.IsNaN(x0) || float.IsNaN(y0) ||
|
float.IsInfinity(vx) || float.IsInfinity(vy) || float.IsInfinity(x0) || float.IsInfinity(y0))
|
{
|
Debug.WriteLine("直线拟合结果包含NaN或Infinity值");
|
return FitLineFallback(points, LineExtension);
|
}
|
|
// 检查方向向量是否为零向量
|
float dirLength = (float)Math.Sqrt(vx * vx + vy * vy);
|
if (dirLength < 1e-6f)
|
{
|
Debug.WriteLine("方向向量长度接近零，使用备用方法");
|
return FitLineFallback(points, LineExtension);
|
}
|
|
// 归一化方向向量
|
vx /= dirLength;
|
vy /= dirLength;
|
|
// 计算直线的起点和终点（基于点集的范围）
|
float minT = float.MaxValue;
|
float maxT = float.MinValue;
|
|
foreach (var point in points)
|
{
|
float t = (point.X - x0) * vx + (point.Y - y0) * vy;
|
if (float.IsNaN(t) || float.IsInfinity(t))
|
continue;
|
|
minT = Math.Min(minT, t);
|
maxT = Math.Max(maxT, t);
|
}
|
|
// 检查参数范围是否有效
|
if (minT > maxT || float.IsNaN(minT) || float.IsNaN(maxT) ||
|
float.IsInfinity(minT) || float.IsInfinity(maxT))
|
{
|
Debug.WriteLine("参数范围无效，使用备用方法");
|
return FitLineFallback(points, LineExtension);
|
}
|
|
// 扩展线段
|
minT -= LineExtension;
|
maxT += LineExtension;
|
|
var startPoint = new Point2f(x0 + minT * vx, y0 + minT * vy);
|
var endPoint = new Point2f(x0 + maxT * vx, y0 + maxT * vy);
|
|
// 检查最终坐标是否有效
|
if (float.IsNaN(startPoint.X) || float.IsNaN(startPoint.Y) ||
|
float.IsNaN(endPoint.X) || float.IsNaN(endPoint.Y))
|
{
|
Debug.WriteLine("最终坐标包含NaN，使用备用方法");
|
return FitLineFallback(points, LineExtension);
|
}
|
|
return new LineSegment(startPoint, endPoint, vx, vy, points);
|
}
|
catch (Exception ex)
|
{
|
Debug.WriteLine($"直线拟合错误: {ex.Message}，使用备用方法");
|
return FitLineFallback(points, LineExtension);
|
}
|
}
|
|
/// <summary>
|
/// 备用直线拟合方法（最小二乘法）
|
/// </summary>
|
private LineSegment FitLineFallback(List<Point2f> points, float LineExtension = 5.0f)
|
{
|
if (points.Count < 2) return null;
|
|
try
|
{
|
// 计算点的均值
|
float meanX = points.Average(p => p.X);
|
float meanY = points.Average(p => p.Y);
|
|
// 计算方差和协方差
|
float varX = 0, varY = 0, covXY = 0;
|
foreach (var point in points)
|
{
|
float dx = point.X - meanX;
|
float dy = point.Y - meanY;
|
varX += dx * dx;
|
varY += dy * dy;
|
covXY += dx * dy;
|
}
|
|
// 检查数据是否有效
|
if (varX < 1e-6f && varY < 1e-6f)
|
{
|
// 所有点几乎重合，创建水平线
|
float minX = points.Min(p => p.X);
|
float maxX = points.Max(p => p.X);
|
float centerY = points.Average(p => p.Y);
|
|
return new LineSegment(new Point2f(minX - LineExtension, centerY)
|
, new Point2f(maxX + LineExtension, centerY), 1, 0, points);
|
}
|
|
// 计算主方向
|
float angle;
|
if (varX > varY)
|
{
|
// 主要沿X轴方向
|
angle = 0;
|
}
|
else
|
{
|
// 主要沿Y轴方向
|
angle = (float)(Math.PI / 2);
|
}
|
|
// 计算方向向量
|
float vx = (float)Math.Cos(angle);
|
float vy = (float)Math.Sin(angle);
|
|
// 计算点在主方向上的投影范围
|
float minT = float.MaxValue;
|
float maxT = float.MinValue;
|
|
foreach (var point in points)
|
{
|
float t = (point.X - meanX) * vx + (point.Y - meanY) * vy;
|
minT = Math.Min(minT, t);
|
maxT = Math.Max(maxT, t);
|
}
|
|
// 扩展线段
|
minT -= LineExtension;
|
maxT += LineExtension;
|
|
var startPoint = new Point2f(meanX + minT * vx, meanY + minT * vy);
|
var endPoint = new Point2f(meanX + maxT * vx, meanY + maxT * vy);
|
|
return new LineSegment(startPoint, endPoint, vx, vy, points);
|
}
|
catch (Exception ex)
|
{
|
Debug.WriteLine($"备用直线拟合也失败: {ex.Message}");
|
return null;
|
}
|
}
|
|
/// <summary>
|
/// 数据预处理，移除异常点
|
/// </summary>
|
private List<Point2f> PreprocessPoints(List<Point2f> points)
|
{
|
if (points.Count < 3) return points;
|
|
// 计算点的中心
|
float meanX = points.Average(p => p.X);
|
float meanY = points.Average(p => p.Y);
|
|
// 计算距离中心的平均距离和标准差
|
var distances = points.Select(p =>
|
Math.Sqrt(Math.Pow(p.X - meanX, 2) + Math.Pow(p.Y - meanY, 2))).ToList();
|
|
double meanDistance = distances.Average();
|
double stdDistance = Math.Sqrt(distances.Average(d => Math.Pow(d - meanDistance, 2)));
|
|
// 移除距离中心过远的异常点（超过3倍标准差）
|
var filteredPoints = points.Where((p, i) =>
|
distances[i] <= meanDistance + 3 * stdDistance).ToList();
|
|
if (filteredPoints.Count < 2)
|
{
|
// 如果过滤后点数太少，返回原始点集
|
return points;
|
}
|
|
Debug.WriteLine($"过滤了 {points.Count - filteredPoints.Count} 个异常点");
|
return filteredPoints;
|
}
|
}
|
}
|
