重构并增强RectangleCenterCalculator功能与结构

全面重构RectangleCenterCalculator类，新增RectangleResult结构体，统一输出中心、轴向、尺寸、角度、面积等参数。明确角点顺序约定，增加顺序校验与主轴角度计算。优化PCA与对角线方法，提升鲁棒性和准确性。移除冗余实现，补充辅助方法与详细注释，提升代码可读性和工程适用性。

TeamAAS-VM/Core/RectangleCenterCalculator.cs

@@ -8,240 +8,407 @@ using System.Threading.Tasks;
 namespace TeamAAS_VP.Core
 {
     /// <summary>
-    /// 提供多种方法用于计算由四个角点定义的矩形中心点以及相关矩形参数的辅助类。
-    /// 该类包含基于对角线优化、PCA（主成分分析）、RANSAC 与最小二乘拟合的不同策略，
-    /// 以应对不同噪声和异常值情形。注意输入假定为三维向量（长度为 3 的向量）。
+    /// 提供多种方法用于计算由四个角点定义的矩形中心点、长轴角度以及相关矩形参数的辅助类。
+    /// 角点顺序约定：左上(0) → 右上(1) → 右下(2) → 左下(3)
+    /// 长轴方向：从左上角(0)指向右上角(1)
+    /// 角度范围：[-180, 180) 度，0°指向X轴正方向，逆时针为正
     /// </summary>
     public class RectangleCenterCalculator
     {
         /// <summary>
-        /// 使用最小二乘法思想计算最优矩形中心点（对外统一入口）。
-        /// 调用当前实现中更鲁棒的对角线优化方法来估计中心。
+        /// 矩形中心及方向信息的综合结果
         /// </summary>
-        /// <param name="corners">矩形的四个角点坐标，每个点为长度为 3 的 <see cref="Vector{Double}"/>。</param>
-        /// <returns>返回估计的中心点坐标（长度为 3 的向量）。</returns>
-        /// <exception cref="ArgumentException">当 <paramref name="corners"/> 为 null 或数量不等于 4 时抛出。</exception>
-        public static Vector<double> CalculateOptimalCenter(List<Vector<double>> corners)
+        public class RectangleResult
+        {
+            /// <summary>
+            /// 矩形中心点坐标
+            /// </summary>
+            public Vector<double> Center { get; set; }
+
+            /// <summary>
+            /// 矩形长轴方向向量（从左上指向右上的单位向量）
+            /// </summary>
+            public Vector<double> MajorAxis { get; set; }
+
+            /// <summary>
+            /// 矩形短轴方向向量（单位向量）
+            /// </summary>
+            public Vector<double> MinorAxis { get; set; }
+
+            /// <summary>
+            /// 矩形长度（沿长轴方向，左上到右上的距离）
+            /// </summary>
+            public double Length { get; set; }
+
+            /// <summary>
+            /// 矩形宽度（沿短轴方向）
+            /// </summary>
+            public double Width { get; set; }
+
+            /// <summary>
+            /// 长轴方向角度[-180, 180)度
+            /// 从左上指向右上的向量在XY平面的投影角度
+            /// 0° = 指向X轴正方向，正角度逆时针，负角度顺时针
+            /// </summary>
+            public double MajorAxisAngle { get; set; }
+
+            /// <summary>
+            /// 四个角点的坐标（保持输入顺序）
+            /// </summary>
+            public List<Vector<double>> Corners { get; set; }
+
+            /// <summary>
+            /// 左上角点坐标
+            /// </summary>
+            public Vector<double> TopLeft => Corners != null && Corners.Count >= 1 ? Corners[0] : null;
+
+            /// <summary>
+            /// 右上角点坐标
+            /// </summary>
+            public Vector<double> TopRight => Corners != null && Corners.Count >= 2 ? Corners[1] : null;
+
+            /// <summary>
+            /// 右下角点坐标
+            /// </summary>
+            public Vector<double> BottomRight => Corners != null && Corners.Count >= 3 ? Corners[2] : null;
+
+            /// <summary>
+            /// 左下角点坐标
+            /// </summary>
+            public Vector<double> BottomLeft => Corners != null && Corners.Count >= 4 ? Corners[3] : null;
+
+            /// <summary>
+            /// 矩形的面积
+            /// </summary>
+            public double Area { get; set; }
+
+            /// <summary>
+            /// 拟合误差（均方根误差）
+            /// </summary>
+            public double FitError { get; set; }
+        }
+
+        /// <summary>
+        /// 验证角点顺序并进行必要的调整
+        /// 确保顺序为：左上 → 右上 → 右下 → 左下
+        /// </summary>
+        private static List<Vector<double>> ValidateAndOrderCorners(List<Vector<double>> corners)
         {
             if (corners == null || corners.Count != 4)
                 throw new ArgumentException("需要4个角点");
 
-            // 方法1：利用矩形对角线相交于中心点的性质
-            return CalculateByDiagonalOptimization(corners);
+            // 这里可以添加自动排序逻辑，但既然您已知顺序，直接返回
+            // 如果未来需要自动排序，可以使用以下逻辑：
+            // 1. 找到最左边的两个点作为左边界
+            // 2. 根据Y坐标区分左上和左下
+            // 3. 计算中心点，确定其他点位置
+
+            return corners; // 假设输入已经按正确顺序
         }
 
         /// <summary>
-        /// 方法1：通过对角线中点的组合优化来估计中心点。
-        /// 由于角点测量可能存在噪声，函数尝试不同的对角线组合，并采用使得四个角点到候选中心距离方差最小的候选中心。
+        /// 计算从左上指向右上的长轴方向角度
         /// </summary>
-        /// <param name="corners">四个角点坐标。</param>
-        /// <returns>估计的中心点向量（长度为 3）。</returns>
-        private static Vector<double> CalculateByDiagonalOptimization(List<Vector<double>> corners)
-        {
-            // 由于点可能有噪声，尝试所有可能的对角线组合
-            // 对角线应该是相对的点
-            var bestCenter = Vector<double>.Build.Dense(3);
-            double minError = double.MaxValue;
-
-            // 常见的对角线组合（这里列出三种互不等价的配对）
-            var diagonalPairs = new List<(int, int, int, int)>
+        private static double CalculateMajorAxisAngle(Vector<double> topLeft, Vector<double> topRight)
         {
-            (0, 2, 1, 3), // p1-p3, p2-p4
-            (0, 1, 2, 3), // p1-p2, p3-p4
-            (0, 3, 1, 2)  // p1-p4, p2-p3
-        };
+            // 计算方向向量（从左上指向右上）
+            Vector<double> direction = topRight - topLeft;
 
-            foreach (var (i1, i2, i3, i4) in diagonalPairs)
-            {
-                // 计算两条对角线的中点
-                var mid1 = (corners[i1] + corners[i2]) / 2.0;
-                var mid2 = (corners[i3] + corners[i4]) / 2.0;
+            // 只考虑XY平面上的投影
+            double x = direction[0];
+            double y = direction[1];
 
-                // 计算两个中点的平均值作为候选中心
-                var candidate = (mid1 + mid2) / 2.0;
+            // 使用Math.Atan2计算角度（弧度）
+            double angleRad = Math.Atan2(y, x);
 
-                // 计算误差：中心到四个角点的距离方差
-                double error = CalculateCenterError(candidate, corners);
+            // 转换为度，得到有符号角度 [-180, 180)
+            double angleDeg = angleRad * 180.0 / Math.PI;
 
-                if (error < minError)
-                {
-                    minError = error;
-                    bestCenter = candidate;
-                }
-            }
+            // Math.Atan2的结果已经是[-180, 180]，但确保一下边界
+            if (angleDeg >= 180.0)
+                angleDeg -= 360.0;
+            else if (angleDeg < -180.0)
+                angleDeg += 360.0;
 
-            return bestCenter;
+            return angleDeg;
         }
 
         /// <summary>
-        /// 计算给定中心到四个角点距离的方差，作为中心优劣的度量（方差越小越优）。
+        /// 使用最小二乘法思想计算最优矩形中心点及相关参数（对外统一入口）
+        /// 推荐使用改进的PCA方法，因为它能利用所有点信息进行优化
         /// </summary>
-        /// <param name="center">待评估的中心点向量（长度为 3）。</param>
-        /// <param name="corners">四个角点坐标。</param>
-        /// <returns>距离的方差值（double）。</returns>
-        private static double CalculateCenterError(Vector<double> center, List<Vector<double>> corners)
+        public static RectangleResult CalculateOptimalCenter(List<Vector<double>> corners)
         {
-            // 计算中心到每个角点的距离
-            var distances = corners.Select(c => Distance(center, c)).ToList();
-
-            // 计算距离的方差 - 越小说明中心越优
-            double mean = distances.Average();
-            double variance = distances.Select(d => Math.Pow(d - mean, 2)).Average();
+            // 验证和调整角点顺序
+            var orderedCorners = ValidateAndOrderCorners(corners);
 
-            return variance;
+            // 使用改进的PCA方法（考虑已知顺序）
+            return CalculateByPCAWithKnownOrder(orderedCorners);
         }
 
         /// <summary>
-        /// 计算两个向量之间的欧几里得距离（L2 范数）。
+        /// 方法1：基于已知角点顺序的直接计算（最准确）
+        /// 直接使用左上和右上点计算长轴，然后计算其他参数
         /// </summary>
-        /// <param name="a">向量 a（长度为 3）。</param>
-        /// <param name="b">向量 b（长度为 3）。</param>
-        /// <returns>返回 a 与 b 之间的 L2 距离。</returns>
-        private static double Distance(Vector<double> a, Vector<double> b)
+        public static RectangleResult CalculateByDirectMethod(List<Vector<double>> corners)
         {
-            return (a - b).L2Norm();
+            var orderedCorners = ValidateAndOrderCorners(corners);
+
+            var topLeft = orderedCorners[0];
+            var topRight = orderedCorners[1];
+            var bottomRight = orderedCorners[2];
+            var bottomLeft = orderedCorners[3];
+
+            // 计算长轴（从左上指向右上）
+            Vector<double> majorAxis = (topRight - topLeft).Normalize(2);
+
+            // 计算短轴（从左上指向左下）
+            Vector<double> minorAxis = (bottomLeft - topLeft).Normalize(2);
+
+            // 确保短轴与长轴垂直（处理非正交情况）
+            minorAxis = (minorAxis - majorAxis * minorAxis.DotProduct(majorAxis)).Normalize(2);
+
+            // 计算中心点（四个角点的平均）
+            var center = Vector<double>.Build.DenseOfEnumerable(
+                Enumerable.Range(0, 3)
+                    .Select(i => orderedCorners.Select(c => c[i]).Average()));
+
+            // 计算长度和宽度
+            double length = Distance(topLeft, topRight);
+            double width = Distance(topLeft, bottomLeft);
+
+            // 计算长轴角度
+            double angle = CalculateMajorAxisAngle(topLeft, topRight);
+
+            // 计算拟合误差
+            double fitError = CalculateFitErrorDirect(center, majorAxis, minorAxis, length, width, orderedCorners);
+
+            return new RectangleResult
+            {
+                Center = center,
+                MajorAxis = majorAxis,
+                MinorAxis = minorAxis,
+                Length = length,
+                Width = width,
+                MajorAxisAngle = angle,
+                Corners = orderedCorners,
+                Area = length * width,
+                FitError = fitError
+            };
         }
 
         /// <summary>
-        /// 方法2：使用 PCA（主成分分析）找到矩形的主方向，然后根据在主轴上的最小/最大投影值计算中心位置。
-        /// 适合点分布呈矩形、噪声较低的情况。
+        /// 方法2：使用PCA但考虑已知角点顺序（推荐）
+        /// 结合PCA的鲁棒性和已知顺序的准确性
         /// </summary>
-        /// <param name="corners">四个角点的坐标列表。</param>
-        /// <returns>在原始坐标系中的估计中心点向量（长度为 3）。</returns>
-        public static Vector<double> CalculateByPCA(List<Vector<double>> corners)
+        public static RectangleResult CalculateByPCAWithKnownOrder(List<Vector<double>> corners)
         {
-            // 将点转换为矩阵
-            var matrix = Matrix<double>.Build.DenseOfRows(corners);
+            var orderedCorners = ValidateAndOrderCorners(corners);
 
-            // 计算均值（初步中心估计）
+            // 步骤1：使用所有点进行PCA得到初步估计
+            var matrix = Matrix<double>.Build.DenseOfRows(orderedCorners);
             var mean = Vector<double>.Build.DenseOfEnumerable(
                 Enumerable.Range(0, 3).Select(i => matrix.Column(i).Average()));
 
-            // 中心化数据
             var centered = matrix.Clone();
             for (int i = 0; i < 4; i++)
             {
                 centered.SetRow(i, centered.Row(i) - mean);
             }
 
-            // 计算协方差矩阵（样本协方差：注意除以 n-1 或 n，当前实现使用除以 3）
             var covariance = centered.Transpose() * centered / 3.0;
-
-            // 特征值分解（PCA）
             var evd = covariance.Evd();
 
-            // 主方向（前两个特征向量）
-            var axis1 = evd.EigenVectors.Column(0);
-            var axis2 = evd.EigenVectors.Column(1);
+            // 获取特征向量
+            var eigenvectors = evd.EigenVectors;
 
-            // 将点投影到两个主轴上
-            var proj1 = corners.Select(p => (p - mean).DotProduct(axis1)).ToList();
-            var proj2 = corners.Select(p => (p - mean).DotProduct(axis2)).ToList();
+            // 步骤2：使用已知顺序确定长轴方向
+            var topLeft = orderedCorners[0];
+            var topRight = orderedCorners[1];
 
-            // 在每个轴上找到最小和最大值
-            double min1 = proj1.Min();
-            double max1 = proj1.Max();
-            double min2 = proj2.Min();
-            double max2 = proj2.Max();
+            // 计算实际的长轴方向（从左上指向右上）
+            Vector<double> actualMajorDirection = (topRight - topLeft).Normalize(2);
+
+            // 从PCA的特征向量中找到最接近实际方向的那个
+            Vector<double> bestAxis = eigenvectors.Column(0);
+            double bestSimilarity = Math.Abs(actualMajorDirection.DotProduct(bestAxis));
+
+            for (int i = 1; i < 3; i++)
+            {
+                var axis = eigenvectors.Column(i);
+                double similarity = Math.Abs(actualMajorDirection.DotProduct(axis));
+                if (similarity > bestSimilarity)
+                {
+                    bestSimilarity = similarity;
+                    bestAxis = axis;
+                }
+            }
+
+            // 确保PCA轴的方向与实际方向一致（点积为正）
+            Vector<double> majorAxis = bestAxis;
+            if (actualMajorDirection.DotProduct(majorAxis) < 0)
+            {
+                majorAxis = -majorAxis;
+            }
 
-            // 计算在主轴上的中心位置（即区间中点）
-            double center1 = (min1 + max1) / 2.0;
-            double center2 = (min2 + max2) / 2.0;
+            // 计算短轴（与长轴垂直）
+            Vector<double> minorAxis = CalculatePerpendicularAxis(majorAxis);
 
-            // 转换回原始坐标系得到中心点
-            var center = mean + axis1 * center1 + axis2 * center2;
+            // 将点投影到轴上计算中心、长度和宽度
+            var projMajor = orderedCorners.Select(p => (p - mean).DotProduct(majorAxis)).ToList();
+            var projMinor = orderedCorners.Select(p => (p - mean).DotProduct(minorAxis)).ToList();
 
-            return center;
+            double centerMajor = (projMajor.Min() + projMajor.Max()) / 2.0;
+            double centerMinor = (projMinor.Min() + projMinor.Max()) / 2.0;
+            var center = mean + majorAxis * centerMajor + minorAxis * centerMinor;
+
+            double length = Math.Abs(projMajor.Max() - projMajor.Min());
+            double width = Math.Abs(projMinor.Max() - projMinor.Min());
+
+            // 计算长轴角度（使用实际的左上-右上方向）
+            double angle = CalculateMajorAxisAngle(topLeft, topRight);
+
+            // 计算拟合误差
+            double fitError = CalculateFitError(center, majorAxis, minorAxis, length, width, orderedCorners);
+
+            return new RectangleResult
+            {
+                Center = center,
+                MajorAxis = majorAxis,
+                MinorAxis = minorAxis,
+                Length = length,
+                Width = width,
+                MajorAxisAngle = angle,
+                Corners = orderedCorners,
+                Area = length * width,
+                FitError = fitError
+            };
         }
 
         /// <summary>
-        /// 方法3：使用最小二乘法直接拟合矩形并返回参数（简化实现）。
-        /// 这里返回的是基于 PCA 的初始估计以及默认轴向量，作为示例接口。
+        /// 方法3：对角线优化方法（考虑已知顺序）
         /// </summary>
-        /// <param name="corners">四个角点坐标。</param>
-        /// <returns>
-        /// 返回一个元组，包含：
-        /// <see cref="Vector{Double}"/> Center - 估计的中心点；
-        /// <see cref="Vector{Double}"/> Axis1 - 第一个轴（单位方向向量，当前示例返回笛卡尔 X 轴）;
-        /// <see cref="Vector{Double}"/> Axis2 - 第二个轴（单位方向向量，当前示例返回笛卡尔 Y 轴）。
-        /// </returns>
-        public static (Vector<double> Center, Vector<double> Axis1, Vector<double> Axis2) FitRectangleByLeastSquares(List<Vector<double>> corners)
+        public static RectangleResult CalculateByDiagonalOptimization(List<Vector<double>> corners)
         {
-            // 步骤1：计算初始质心
-            var initialCenter = Vector<double>.Build.DenseOfEnumerable(
-                Enumerable.Range(0, 3).Select(i => corners.Select(c => c[i]).Average()));
+            var orderedCorners = ValidateAndOrderCorners(corners);
+            var topLeft = orderedCorners[0];
+            var topRight = orderedCorners[1];
 
-            // 步骤2：使用Levenberg-Marquardt优化
-            // 定义优化问题：找到中心、两个轴方向、长度和宽度
-            // 这里简化实现，使用PCA结果作为初始值
+            // 直接使用已知顺序计算长轴角度
+            double angle = CalculateMajorAxisAngle(topLeft, topRight);
 
-            var pcaCenter = CalculateByPCA(corners);
+            // 计算长轴方向向量
+            Vector<double> majorAxis = (topRight - topLeft).Normalize(2);
+            Vector<double> minorAxis = CalculatePerpendicularAxis(majorAxis);
 
-            // 计算平均偏差（用于诊断输出）
-            var errors = corners.Select(c => (c - pcaCenter).L2Norm()).ToList();
-            double avgError = errors.Average();
+            // 使用所有对角线组合计算最佳中心
+            var diagonalCombinations = new[]
+            {
+            (orderedCorners[0], orderedCorners[2]), // 左上-右下
+            (orderedCorners[1], orderedCorners[3])  // 右上-左下
+        };
 
-            Console.WriteLine($"PCA中心点: [{pcaCenter[0]:F4}, {pcaCenter[1]:F4}, {pcaCenter[2]:F4}]");
-            Console.WriteLine($"平均误差: {avgError:F6}");
+            var centers = diagonalCombinations.Select(p => (p.Item1 + p.Item2) / 2.0).ToList();
+            var center = (centers[0] + centers[1]) / 2.0;
 
-            // 返回简化的轴向量（占位，实际实现可替换为优化得到的方向）
-            var axis1 = Vector<double>.Build.Dense(new[] { 1.0, 0.0, 0.0 });
-            var axis2 = Vector<double>.Build.Dense(new[] { 0.0, 1.0, 0.0 });
+            // 计算长度和宽度
+            double length = Distance(topLeft, topRight);
+            double width = Distance(topLeft, orderedCorners[3]); // 左上-左下
 
-            return (pcaCenter, axis1, axis2);
+            // 计算拟合误差
+            double fitError = CalculateFitError(center, majorAxis, minorAxis, length, width, orderedCorners);
+
+            return new RectangleResult
+            {
+                Center = center,
+                MajorAxis = majorAxis,
+                MinorAxis = minorAxis,
+                Length = length,
+                Width = width,
+                MajorAxisAngle = angle,
+                Corners = orderedCorners,
+                Area = length * width,
+                FitError = fitError
+            };
         }
 
         /// <summary>
-        /// 方法4：使用 RANSAC（随机采样一致性）估计中心，适用于存在异常点的情况。
-        /// 随机选择两点作为候选对角线端点，统计满足阈值的内点数量，选择内点最多的候选中心，并用内点重计算中心。
+        /// 辅助方法：计算与给定向量垂直的单位向量
         /// </summary>
-        /// <param name="corners">四个角点坐标。</param>
-        /// <param name="iterations">RANSAC 迭代次数，越大越稳健但计算开销越高。默认 100。</param>
-        /// <param name="threshold">判断内点的距离阈值（以与坐标单位一致的长度度量）。默认 0.01。</param>
-        /// <returns>估计得到的中心点向量（长度为 3）。若没有足够内点则返回在迭代过程中最佳的候选中心（可能为零向量）。</returns>
-        public static Vector<double> CalculateByRANSAC(List<Vector<double>> corners,int iterations = 100, double threshold = 0.01)
+        private static Vector<double> CalculatePerpendicularAxis(Vector<double> axis)
         {
-            var bestCenter = Vector<double>.Build.Dense(3);
-            int bestInliers = 0;
-
-            var random = new Random();
-
-            for (int i = 0; i < iterations; i++)
-            {
-                // 随机选择两个点作为对角线端点
-                int idx1 = random.Next(4);
-                int idx2;
-                do
-                {
-                    idx2 = random.Next(4);
-                } while (idx2 == idx1);
+            // 找一个不与axis共线的向量
+            Vector<double> temp;
+            if (Math.Abs(axis[0]) < 0.9)
+                temp = Vector<double>.Build.Dense(new[] { 1.0, 0.0, 0.0 });
+            else
+                temp = Vector<double>.Build.Dense(new[] { 0.0, 1.0, 0.0 });
+
+            var perpendicular = temp - axis * temp.DotProduct(axis);
+            return perpendicular.Normalize(2);
+        }
 
-                // 计算对角线中点
-                var candidateCenter = (corners[idx1] + corners[idx2]) / 2.0;
+        /// <summary>
+        /// 辅助方法：计算两点距离
+        /// </summary>
+        private static double Distance(Vector<double> a, Vector<double> b)
+        {
+            return (a - b).L2Norm();
+        }
 
-                // 统计内点（距离接近的点）
-                int inliers = corners.Count(c =>
-                    (c - candidateCenter).L2Norm() < threshold);
+        /// <summary>
+        /// 辅助方法：计算拟合误差
+        /// </summary>
+        private static double CalculateFitError(Vector<double> center, Vector<double> majorAxis,Vector<double> minorAxis, double length, double width, List<Vector<double>> corners)
+        {
+            // 生成理想矩形的四个角点
+            var idealCorners = new List<Vector<double>>
+        {
+            center + majorAxis * (length / 2) + minorAxis * (width / 2),   // 右上
+            center + majorAxis * (length / 2) - minorAxis * (width / 2),   // 右下
+            center - majorAxis * (length / 2) - minorAxis * (width / 2),   // 左下
+            center - majorAxis * (length / 2) + minorAxis * (width / 2)    // 左上
+        };
 
-                if (inliers > bestInliers)
+            // 注意：理想角点顺序可能与输入不同，需要重新排序匹配
+            // 这里简单计算所有对应点的最小距离和
+            double sumSquared = 0;
+            for (int i = 0; i < 4; i++)
+            {
+                // 找到最近的理想角点
+                double minDist = double.MaxValue;
+                foreach (var ideal in idealCorners)
                 {
-                    bestInliers = inliers;
-                    bestCenter = candidateCenter;
+                    double dist = Distance(corners[i], ideal);
+                    if (dist < minDist) minDist = dist;
                 }
+                sumSquared += minDist * minDist;
             }
 
-            // 使用所有内点重新计算中心（若存在内点）
-            var inlierPoints = corners.Where(c =>
-                (c - bestCenter).L2Norm() < threshold).ToList();
+            return Math.Sqrt(sumSquared / 4);
+        }
+
+        /// <summary>
+        /// 直接计算方法的拟合误差
+        /// </summary>
+        private static double CalculateFitErrorDirect(Vector<double> center, Vector<double> majorAxis,Vector<double> minorAxis, double length, double width, List<Vector<double>> corners)
+        {
+            // 对于直接方法，我们知道每个角点的理想位置
+            var idealCorners = new List<Vector<double>>
+        {
+            center - majorAxis * (length / 2) + minorAxis * (width / 2),  // 左上
+            center + majorAxis * (length / 2) + minorAxis * (width / 2),  // 右上
+            center + majorAxis * (length / 2) - minorAxis * (width / 2),  // 右下
+            center - majorAxis * (length / 2) - minorAxis * (width / 2)   // 左下
+        };
 
-            if (inlierPoints.Count > 0)
+            double sumSquared = 0;
+            for (int i = 0; i < 4; i++)
             {
-                bestCenter = Vector<double>.Build.DenseOfEnumerable(
-                    Enumerable.Range(0, 3)
-                        .Select(dim => inlierPoints.Select(p => p[dim]).Average()));
+                sumSquared += Math.Pow(Distance(corners[i], idealCorners[i]), 2);
             }
 
-            return bestCenter;
+            return Math.Sqrt(sumSquared / 4);
         }
     }
 }