角点自动排序与鲁棒性提升

实现角点自动排序为标准矩形顺序，提升鲁棒性。新增排序相关辅助方法，采用上下分组+水平排序启发式算法并引入评分机制自动选择最优排序。优化注释，增加调试打印（已注释）以便开发调试。

TeamAAS-VM/Core/RectangleCenterCalculator.cs

@@ -97,13 +97,7 @@ namespace TeamAAS_VP.Core
             if (corners == null || corners.Count != 4)
                 throw new ArgumentException("需要4个角点");
 
-            // 这里可以添加自动排序逻辑，但既然您已知顺序，直接返回
-            // 如果未来需要自动排序，可以使用以下逻辑：
-            // 1. 找到最左边的两个点作为左边界
-            // 2. 根据Y坐标区分左上和左下
-            // 3. 计算中心点，确定其他点位置
-
-            return corners; // 假设输入已经按正确顺序
+            return SortCornersByTopBottomHeuristic(corners);
         }
 
         /// <summary>
@@ -204,7 +198,20 @@ namespace TeamAAS_VP.Core
         /// </summary>
         public static RectangleResult CalculateByPCAWithKnownOrder(List<Vector<double>> corners)
         {
+            //打印输入点以调试
+            //Console.WriteLine($"输入点 [1] : {corners[0][0]:F3}，{corners[0][1]:F3}");
+            //Console.WriteLine($"输入点 [2] : {corners[1][0]:F3}，{corners[1][1]:F3}");
+            //Console.WriteLine($"输入点 [3] : {corners[2][0]:F3}，{corners[2][1]:F3}");
+            //Console.WriteLine($"输入点 [4] : {corners[3][0]:F3}，{corners[3][1]:F3}");
+
             var orderedCorners = ValidateAndOrderCorners(corners);
+            Console.WriteLine($"");
+
+            //打印输入点以调试
+            //Console.WriteLine($"输出点 [1] : {orderedCorners[0][0]:F3}，{orderedCorners[0][1]:F3}");
+            //Console.WriteLine($"输出点 [2] : {orderedCorners[1][0]:F3}，{orderedCorners[1][1]:F3}");
+            //Console.WriteLine($"输出点 [3] : {orderedCorners[2][0]:F3}，{orderedCorners[2][1]:F3}");
+            //Console.WriteLine($"输出点 [4] : {orderedCorners[3][0]:F3}，{orderedCorners[3][1]:F3}");
 
             int dim = orderedCorners[0].Count;
             // 步骤1：使用所有点进行PCA得到初步估计
@@ -343,7 +350,7 @@ namespace TeamAAS_VP.Core
             int n = axis.Count;
             if (n == 0) throw new ArgumentException("axis must have positive dimension", nameof(axis));
 
-            // 专门处理2D：(-y, x) 是垂直向量
+            // 2D场景：(-y, x) 为垂直向量
             if (n == 2)
             {
                 var perp2 = Vector<double>.Build.DenseOfArray(new[] { -axis[1], axis[0] });
@@ -351,7 +358,7 @@ namespace TeamAAS_VP.Core
                 return perp2.Normalize(2);
             }
 
-            // 一般n维：选一个与axis不共线的标准基向量
+            // 通用n维：选一个不与axis共线的标准基向量
             int idx = 0;
             for (int i = 0; i < n; i++)
             {
@@ -426,5 +433,91 @@ namespace TeamAAS_VP.Core
 
             return Math.Sqrt(sumSquared / 4);
         }
+
+        /// <summary>
+        /// 自动排序角点：左上 → 右上 → 右下 → 左下
+        /// </summary>
+        /// <param name="unorderedCorners">任意顺序的四个角点</param>
+        /// <returns>按标准顺序排序的角点列表</returns>
+        public static List<Vector<double>> SortCornersToRectangleOrder(List<Vector<double>> unorderedCorners)
+        {
+            if (unorderedCorners == null || unorderedCorners.Count != 4)
+                throw new ArgumentException("需要4个角点");
+
+            // 方法1：基于上下分组+水平排序的稳健方法
+            return SortCornersByTopBottomHeuristic(unorderedCorners);
+        }
+
+        /// <summary>
+        /// 方法1：基于上下分组与水平排序的稳健方法
+        /// </summary>
+        private static List<Vector<double>> SortCornersByTopBottomHeuristic(List<Vector<double>> corners)
+        {
+            if (corners == null || corners.Count != 4)
+                throw new ArgumentException("需要4个角点");
+
+            var orderHighY = BuildOrderByTopBottom(corners, topIsHigher: true);
+            var orderLowY = BuildOrderByTopBottom(corners, topIsHigher: false);
+
+            double scoreHigh = RectangleOrderScore(orderHighY);
+            double scoreLow = RectangleOrderScore(orderLowY);
+
+            return scoreHigh <= scoreLow ? orderHighY : orderLowY;
+        }
+
+        private static List<Vector<double>> BuildOrderByTopBottom(List<Vector<double>> corners, bool topIsHigher)
+        {
+            var sortedByY = topIsHigher
+                ? corners.OrderByDescending(p => p[1]).ToList()
+                : corners.OrderBy(p => p[1]).ToList();
+
+            var topCandidates = sortedByY.Take(2).ToList();
+            var bottomCandidates = sortedByY.Skip(2).Take(2).ToList();
+
+            var top = topCandidates.OrderBy(p => p[0]).ToList();
+            var bottom = bottomCandidates.OrderBy(p => p[0]).ToList();
+
+            var topLeft = top[0];
+            var topRight = top[1];
+            var bottomLeft = bottom[0];
+            var bottomRight = bottom[1];
+
+            return new List<Vector<double>> { topLeft, topRight, bottomRight, bottomLeft };
+        }
+
+        private static double RectangleOrderScore(List<Vector<double>> ordered)
+        {
+            if (ordered == null || ordered.Count != 4)
+                return double.MaxValue;
+
+            var topLeft = ordered[0];
+            var topRight = ordered[1];
+            var bottomRight = ordered[2];
+            var bottomLeft = ordered[3];
+
+            double top = Distance(topLeft, topRight);
+            double bottom = Distance(bottomLeft, bottomRight);
+            double left = Distance(topLeft, bottomLeft);
+            double right = Distance(topRight, bottomRight);
+            double diag1 = Distance(topLeft, bottomRight);
+            double diag2 = Distance(topRight, bottomLeft);
+
+            double parallelScore = Math.Abs(top - bottom) + Math.Abs(left - right) + Math.Abs(diag1 - diag2);
+
+            double xOrderPenalty = 0.0;
+            if (topLeft[0] > topRight[0]) xOrderPenalty += 1000.0;
+            if (bottomLeft[0] > bottomRight[0]) xOrderPenalty += 1000.0;
+
+            return parallelScore + xOrderPenalty;
+        }
+
+        /// <summary>
+        /// 调整角点顺序为标准矩形顺序（左上→右上→右下→左下）
+        /// </summary>
+        private static List<Vector<double>> AdjustToRectangleOrder(List<Vector<double>> corners)
+        {
+            return SortCornersByTopBottomHeuristic(corners);
+        }
+
     }
 }