前言

        本篇是二轮实战number1，对随机一致性算法进行深度剖析。

        关于此篇，小编基于两组数据--三维点＆时间序列，利用两大类体，基于测量学常熟数据处理方案，实现平面拟合＆曲线拟合两大功能。这是继简单的线性拟合之后的一次拓展研究，用更复杂的数据实现更复杂的拟合，在这个过程中，更进一步加深对随机一致性的理解。关于线性实现，家人感兴趣可移步至下方链接👇

从噪声中寻找真理：RANSAC算法在测绘程序设计大赛中的实战指南（2025国赛选题一）

一、国赛选题＆本题要求

（一）六大选题

（二）本题要求

        在一组含有粗差的数据中，循环进行随机抽样和一致性评估，实现参数的稳健估计。

（三）一般步骤＆注意事项

        写了很多关于今年国赛选题的讨论文章，对各大选题也手搓过很多不同的实现方案，现在其实已经有一种思维定势存在，我觉得不管拿到什么题，以下这几步，不管题出成什么鬼样，都是可以先无脑做的。

• 设计窗体：
  • tabcontrol、menuStrip、statusStrip、dataGridView、toolstrip、label、richTextBox，这几个控件，get基本上就能设计出来一个不错的窗体。
  • 窗体的主体一般至少包含两部分：原始数据读取后的展示（用表格、富文本框都OK），结果报告的展示（按照试题册，把结果输出到窗体）
  • 当然还有根据题目设置的各种button_click
• 设计数据结构类：
  • 根据给的数据，赋予其基本属性。
  • 根据输出结果要求，赋予其我们需要经过计算得到的属性
  • ···
• 设计文件管理类：
  • 一般至少实现两个方法：文件读取、结果保存。
• 注意事项：
  • 无论什么语言，不可以使用第三方库。赛前可以多看看相关要求，避免赛时因疏忽扣大分。
  • 无论文档还是源码文件，必须在开赛时才开始动手，到时会判断文档以及源码生成时间，请注意不要急。
  • 可视化绘图，已经明确了不做要求，重点应该放在实现算法和保证结果正确且规范上，时间评分项（大家可以去b站看第一次直播回放，里面有讲到今年的评分标准）。

二、数据＆算法解析

        //注：此篇基于的文档已上传至资源区，是我的老师写的，各位家人可参考。

（一）本题分析

        明确了用到随机一致性算法，这道题还是善良一点的。和GNSS、点云去噪一样，这道题也是属于数据预处理问题。

        赛题文件中给的要求是什么意思，就是说我们会拿到一组（也可能是多组）含有粗差的数据（就是存在问题的数据），我们要利用随机一致性算法的思想，从已有的数据中不断的循环抽样、对试题册中给的拟合公式（就是表征这组数据中关系的公式）的未知的参数进行计算，估计，迭代，更新，再循环，直到我们得到的参数估计值能够做到让我们用公式计算得到的数据，和我们的样本数据的偏差，小于给定阈值。简单说，我们要实现的，本质思想就是概率论里的参数估计问题 哈哈哈，我们知道两者相关，但不知道因何相关。我们要做的就是找到尽可能可以表征两者相互关系的那些参数，让这个相关公式完整。

（二）数据

        本篇基于两组数据，在实现了直线拟合之后，现在我们来看看如何实现平面和曲线。

• 用于平面拟合的数据
  • 这个数据还是很善良的，从左到右依次为id,x,y,z，就是三维常规数据。

• 用于曲线拟合的数据
  • 嘶，这个数据就有点抽象了哈哈。
  • Time：时间（年，小数形式），作为曲线拟合公式里的的自变量t
  • north、east、vert：三个方向的位移数据，作为拟合模型的因变量y（分别对应北、东、垂直方向的观测值）；
  • 其他字段（sn、se、su 等）为误差或相关性参数，不参与曲线拟合核心计算（小编了解到这些误差和相关系数在更高层次的数据处理上也是必须的，但不考虑也不会影响拟合的实现，比赛的时候，不会在数据处理上让选手花费太多时间的，算法比整理数据更核心，所以这里可以直接略过，当然大家也可以试试哈哈）。

（三）平面拟合

         “数据准备 → 随机采样 → 模型构建 → 内点评估 → 迭代优化 → 输出最佳模型”

• 数据准备
  • 先定义好数据结构模型，就直接按照.txt里的字段进行定义就OK，不涉及额外字段
  • 封装到列表，便于处理
• 随机采样
  • 每次随机抽样三个点（平面拟合样本点为3，直线2）
  • 进行共线检测，共线（海伦公式求得的面积小于阈值）则舍弃，进行重采样。

• 模型构建
  • 用样本点来求解平面模型参数

                

• 用得到的模型求解内点集
  • 点到平面距离，小于阈值则判定为内点

• 迭代优化
  • 不断循环抽样，不断重复上述操作，求最佳参数
• 终止条件：
  • 迭代次数达上限
  • 模型包含内点已经达到预期标准（已经能够近乎真值的表征实测值了）

（四）曲线拟合

    “数据初始化 → 设计矩阵构建 → 参数求解 →残差分析与粗差剔除 →迭代优化 → 最终拟合”

• 模型定义
  • 这里用摒弃粗差探测后的实现公式（多周期谐波拟合模型简化版哈哈）。

• 设计矩阵
  • 定义一个包含八个元素的一维数组，各元素值就按照公式中定义

• 参数求解（最小二乘法⭐）
  • 思想：“让模型预测值和真实值的误差平方和最小”
  • 过程：
    • 构造方程
    • 用高斯约旦消元法求解，得参数估计值（就是公式里的A0···）
    • （详细过程见代码部分，做了详细注释，欢迎家人们在评论区讨论，也欢迎私信小编一起探讨）
• 残差估计＆粗差剔除＆模型优化
  • 循环做：用当前模型算残差 → 找残差超过 3σ 的点 → 剔除后重新拟合 → 直到没啥异常点或者迭代次数够了。
• 保留最终拟合结果

（五）总体思路

        平面：

• 构建数据模型，把平面点导进来
• 循环随机抽样，一次三个（设置停止标志）
• 判断是否共线 ，共线 ->continue；
• 按公式求参数构建模型
• 基于模型求内点集
• 继续循环优化，直到达到预期

        曲线：

• 第一步依旧是首先把数据导进来
• 对每一个方向，先构建设计矩阵（按公式，8个参数），构建起矩阵方程
• 进行最小二乘求解
• 进行残差估计，剔除粗差（就是和模型之差远大于一般水平的那些数据，就是有问题的），然后再用剔除粗差之后的样本集，再进行模型拟合
• 循环迭代上述步骤，直到达到预期or循环上限。

三、C#实现

//小编这一次直接把代码粘了过来，家人们觉得图片更舒服还是这样直接粘代码更舒服呢，小编个人觉得还是图片占地小，翻起来不会那么有篇幅长度压力哈哈哈哈。

• 平面

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static RansacTeacherXiao_V1.DataModel;
using System.Windows.Forms;
using System.IO;


namespace RansacTeacherXiao_V1
{
    /// <summary>
    /// 平面拟合
    /// </summary>
    public static class PlaneFitter
    {


        #region 平面拟合入口

        public static PlaneResult FitPlane(int iterations = 1000,double threshold = 0.1)
        {
            if (DataCenter.points.Count < 3)
            {
                MessageBox.Show("至少需要三个点才能进行平面拟合");
                return null;
            }
            PlaneResult bestPlane = null;
            Random random = new Random();

            for(int i = 0; i < iterations; i++)
            {
                //1、随机选择三个点
                var sample = GetRandomSample(random);

                //2、检查是否共线
                if (ArePointCollinear(sample[0], sample[1], sample[2]))
                    continue;

                //3、计算平面参数
                var plane = CalculatePlane(sample);

                //4、评估平面
                int inlierCount = CountInliers(plane, threshold);

                //5、更新最佳平面
                if(bestPlane == null || inlierCount > bestPlane.InlierCount)
                {
                    bestPlane = plane;
                    bestPlane.InlierCount = inlierCount;
                    bestPlane.Inliers = GetInliers(plane, threshold);
                }
            }

            StringBuilder planeReport = new StringBuilder();
            planeReport.AppendLine("----------平面拟合报告-----------");
            planeReport.AppendLine(bestPlane.ToString());

            DataCenter.planeReports = planeReport.ToString();

            return bestPlane;
        }


        #endregion



        #region 拟合辅助

        //随机选择三个点
        public static List<Points> GetRandomSample(Random random)
        {
            var sample = new List<Points>();
            HashSet<int> indices = new HashSet<int>();

            while(indices.Count < 3)
            {
                int index = random.Next(0, DataCenter.points.Count);
                if (!indices.Contains(index))
                {
                    indices.Add(index);
                    sample.Add(DataCenter.points[index]);
                }
            }
            return sample;
        }

        //判断是否共线
        public static bool ArePointCollinear(Points p1,Points p2,Points p3)
        {
            double a = Distance(p1, p1);
            double b = Distance(p2, p3);
            double c = Distance(p3, p1);
            double s = (a + b + c) / 2;
            double area = Math.Sqrt(s * (s - a) * (s - b) * (s - c));
            return area < 0.1;
        }

        //求两点距离
        public static double Distance(Points p1,Points p2)
        {
            double dx = p2.x - p1.x;
            double dy = p2.y - p1.y;
            double dz = p2.z - p1.z;
            return Math.Sqrt(dx * dx + dy * dy + dz * dz);

        }

        //计算平面参数
        public static PlaneResult CalculatePlane(List<Points> points)
        {
            if (points.Count != 3)
            {
                throw new Exception("需要三个点来计算平面");
            }

            double x1 = points[0].x, y1 = points[0].y, z1 = points[0].z;
            double x2 = points[1].x, y2 = points[1].y, z2 = points[1].z;
            double x3 = points[2].x, y3 = points[2].y, z3 = points[2].z;

            double v1x = x2 - x1;
            double v1y = y2 - y1;
            double v1z = z2 - z1;

            double v2x = x3 - x1;
            double v2y = y3 - y1;
            double v2z = z3 - z1;

            double normalX = v1y * v2z - v1z * v2y;
            double normalY = v1z * v2x - v1x * v2z;
            double normalZ = v1x * v2y - v1y * v2x;

            double magnitude = Math.Sqrt(normalX * normalX + normalY * normalY + normalZ * normalZ);
            normalX /= magnitude;
            normalY /= magnitude;
            normalZ /= magnitude;

            double A = normalX;
            double B = normalY;
            double C = normalZ;
            double D = -(A * x1 + B * y1 + C * z1);

            return new PlaneResult { A = A, B = B, C = C, D = D };

        }

        //计算点到平面距离
        public static double PointToPlaneDistance(PlaneResult plane,Points point)
        {
            return Math.Abs(plane.A * point.x + plane.B * point.y + plane.C * point.z + plane.D) /
                Math.Sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
        }


        //统计内点数量
        public static int CountInliers(PlaneResult plane,double threshold)
        {
            int count = 0;

            foreach(var point in DataCenter.points)
            {
                if (PointToPlaneDistance(plane, point) < threshold)
                {
                    count++;
                }

            }
            return count;
        }

        public static List<Points> GetInliers(PlaneResult plane,double threshold)
        {
            var inliers = new List<Points>();
            foreach(var point in DataCenter.points)
            {
                if (PointToPlaneDistance(plane, point) < threshold)
                    inliers.Add(point);
            }
            return inliers;
        }

        #endregion


        #region 平面拟合结果构造
        public class PlaneResult
        {
            public double A { get; set; }
            public double B { get; set; }
            public double C { get; set; }
            public double D { get; set; }

            public int InlierCount { get; set; }

            public List<Points> Inliers { get; set; } = new List<Points>();

            public override string ToString()
            {
                return $"平面方程: {A:F6}x + {B:F6}y + {C:F6}z + {D:F6} = 0\n内部点数量: {InlierCount}";
            }
        }

        #endregion

    }
}

• 曲线

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static RansacTeacherXiao_V1.DataModel;
using System.Windows.Forms;
using System.IO;

namespace RansacTeacherXiao_V1
{
    /// <summary>
    /// 曲线拟合模块 - 对时间序列数据进行多周期谐波拟合
    /// </summary>
    public static class CurveFitter
    {

        #region 曲线拟合入口  

        /// <summary>
        /// ⭐对时间序列数据进行多周期曲线拟合
        /// </summary>
        public static List<CurveResult> FitCurves(int maxTterations = 10,double sigmaThreshold = 3.0)
        {
            if(DataCenter.tskbs.Count == 0)
            {
                MessageBox.Show("无可用数据，请检查是否正确导入");
                return null;
            }

            var results = new List<CurveResult>();
            string[] directions = { "north", "east", "vert" };

            foreach(string direction in directions)
            {
                try
                {
                    var result = FitSingleDirection(direction, maxTterations, sigmaThreshold);
                    results.Add(result);
                }
                catch
                {
                    throw new Exception($"{direction}方向拟合出错");
                }
            }

            StringBuilder curveReport = new StringBuilder();
            curveReport.AppendLine("-----------曲线拟合报告-----------");
            foreach (var result in results)
            {
                curveReport.AppendLine(result.ToString());
            }

            DataCenter.curveReports = curveReport.ToString();
            return results;
        }

        #endregion


        #region 曲线拟合辅助

        /// <summary>
        /// 对单个方向的数据进行曲线拟合
        /// </summary>
        public static CurveResult FitSingleDirection(string direction,int maxIterations,double sigmaThreshold)
        {
            bool[] mask = new bool[DataCenter.tskbs.Count];
            for(int i =0;i<mask.Length;i++)
            {
                mask[i] = true;
            }

            int iteration = 0;//迭代次数
            bool hasOutliers = true;

            while (iteration < maxIterations)
            {
                iteration++;
                hasOutliers = false;

                List<double> times = new List<double>();
                List<double> values = new List<double>();

                for(int i = 0; i < DataCenter.tskbs.Count; i++)
                {
                    if (mask[i])
                    {
                        times.Add(DataCenter.tskbs[i].time);
                        values.Add(GetValueByDirection(DataCenter.tskbs[i], direction));
                    }
                }

                if (times.Count < 8)
                {
                    MessageBox.Show("数据点不足,至少需要8个数据点！");
                    break;
                }

                double[][] designMatrix = new double[times.Count][];
                for(int i = 0; i < times.Count; i++)
                {
                    double t = times[i];
                    designMatrix[i] = new double[8]
                    {
                        1,  //常数项            
                        t,  //线性趋势
                        Math.Cos(2*Math.PI*t),//年周期余弦
                        Math.Sin(2*Math.PI*t),//年周期正弦
                        Math.Cos(4*Math.PI*t),//半年周期余弦
                        Math.Sin(4*Math.PI*t),//半年周期正弦
                        Math.Cos(8*Math.PI*t),//四分之一年周期余弦
                        Math.Sin(8*Math.PI*t)//四分之一年周期正弦
                    };
                }
                double[] parameters = LeastSquareSolve(designMatrix, values.ToArray());

                double[] residuals = new double[times.Count];
                for(int i = 0; i < times.Count; i++)
                {
                    double fittedValue = 0;
                    for(int j = 0; j < 8; j++)
                    {
                        fittedValue += designMatrix[i][j] * parameters[j];
                    }
                    residuals[i] = values[i] - fittedValue;
                }

                double meanResidual = residuals.Average();
                double variance = residuals.Sum(r => Math.Pow(r - meanResidual, 2)) / (residuals.Length - 1);
                double sigma = Math.Sqrt(variance);

                int outlierCount = 0;
                int validIndex = 0;
                for (int i = 0; i < DataCenter.tskbs.Count; i++)
                {
                    if (mask[i])
                    {
                        if (Math.Abs(residuals[validIndex]) > sigmaThreshold * sigma)
                        {
                            mask[i] = false;
                            hasOutliers = true;
                            outlierCount++;
                        }
                        validIndex++;
                    }

                }
                if (outlierCount > 0)
                {
                    Console.WriteLine($"第{iteration}次迭代，{direction}方向上探测到{outlierCount}个粗差");
                }
            }

            var cleanData = new List<TSKB>();
            for(int i = 0; i < DataCenter.tskbs.Count; i++)
            {
                if (mask[i])
                    cleanData.Add(DataCenter.tskbs[i]);
            }

            List<double> finalTimes = new List<double>();
            List<double> finalValues = new List<double>();

            foreach(var tskb in cleanData)
            {
                finalTimes.Add(tskb.time);
                finalValues.Add(GetValueByDirection(tskb, direction));
            }

            double[][] finalDesignMatrix = new double[finalTimes.Count][];
            for(int i = 0; i < finalTimes.Count; i++)
            {
                double t = finalTimes[i];
                finalDesignMatrix[i] = new double[8]
                {
                    1,                  // 常数项
                    t,                  // 线性趋势
                    Math.Cos(2 * Math.PI * t),  // 年周期余弦
                    Math.Sin(2 * Math.PI * t),  // 年周期正弦
                    Math.Cos(4 * Math.PI * t),  // 半年周期余弦
                    Math.Sin(4 * Math.PI * t),  // 半年周期正弦
                    Math.Cos(8 * Math.PI * t),  // 四分之一年周期余弦
                    Math.Sin(8 * Math.PI * t)   // 四分之一年周期正弦
                };
            }

            double[] finalParameters = LeastSquareSolve(finalDesignMatrix, finalValues.ToArray());

            return new CurveResult
            {
                Direction = direction,
                A0 = finalParameters[0],
                A1 = finalParameters[1],
                A2 = finalParameters[2],
                B2 = finalParameters[3],
                A3 = finalParameters[4],
                B3 = finalParameters[5],
                A4 = finalParameters[6],
                B4 = finalParameters[7],
                CleanData = cleanData
            };
        }
        
        /// <summary>
        /// 根据方向获取对应值
        /// </summary>
        public static double GetValueByDirection(TSKB tskb,string direction)
        {
            switch (direction)
            {
                case "north": return tskb.north;
                case "east": return tskb.east;
                case "vert": return tskb.vert;
                default: throw new ArgumentException("无效的方向: " + direction);

            }
        }

        /// <summary>
        /// 最小二乘求解
        /// </summary>
        public static double[] LeastSquareSolve(double[][] designMatrix,double[] obs)
        {
            int n = designMatrix.Length;//观测数
            int m = designMatrix[0].Length;//参数数

            double[][] at = Transpose(designMatrix);
            double[][] ata = MultiplyAB(at, designMatrix);
            double[] atb = MultiplyAb(at, obs);

            return SolveLinearSystem(ata, atb);
        }


        /// <summary>
        /// 矩阵转置
        /// </summary>
        public static double[][] Transpose(double[][] matrix)
        {
            int rows = matrix.Length;
            int cols = matrix[0].Length;

            double[][] result = new double[cols][];

            for(int i =0;i<cols; i++)
            {
                result[i] = new double[rows];
                for(int j = 0; j < rows; j++)
                {
                    result[i][j] = matrix[j][i];
                }
            }

            return result;
        }

        /// <summary>
        /// 矩阵乘法 (A*B)
        /// </summary>
        public static double[][] MultiplyAB(double[][] a, double[][] b)
        {
            int rowsA = a.Length;
            int colsA = a[0].Length;
            int colsB = b[0].Length;

            double[][] result = new double[rowsA][];
            for(int i = 0; i < rowsA; i++)
            {
                result[i] = new double[colsB];
                for(int j = 0; j < colsB; j++)
                {
                    for(int k = 0; k < colsA; k++)
                    {
                        result[i][j] += a[i][k] * b[k][j];
                    }
                }
            }

            return result;
        }


        /// <summary>
        /// 矩阵与向量乘法(A*b) 
        /// </summary>
        public static double[] MultiplyAb(double[][] a, double[] b)
        {
            int rows = a.Length;
            int cols = a[0].Length;

            double[] result = new double[rows];

            for(int i = 0; i < rows; i++)
            {
                for(int j = 0; j < cols; j++)
                {
                    result[i] += a[i][j] * b[j];
                }
            }
            return result;
        }


        /// <summary>
        /// 使用高斯消元法求解线性方程组 Ax = b
        /// </summary>
        public static double[] SolveLinearSystem(double[][] a, double[] b)
        {
            int n = a.Length;
            double[][] augmented = new double[n][];


            //1、构建增广矩阵
            for(int i = 0; i < n; i++)
            {
                augmented[i] = new double[n + 1];
                for(int j = 0; j < n; j++)
                {
                    augmented[i][j] = a[i][j];
                }
                augmented[i][n] = b[i];
            }


            //2、前向消元--造上三角矩阵
            for(int i = 0; i < n; i++)
            {

                //2.1 锁定主元--当前列的元素值绝对值最大的行
                int maxRow = i;
                for(int k = i + 1; k < n; k++)
                {
                    if (Math.Abs(augmented[k][i]) > Math.Abs(augmented[maxRow][i]))
                    {
                        maxRow = k;
                    }
                }

                //2.2 交换行--交换当前行i和主元行maxRow

                double[] temp = augmented[i];
                augmented[i] = augmented[maxRow];
                augmented[maxRow] = temp;


                //2.3 检查主元是否为0（为0则无解）
                if (Math.Abs(augmented[i][i]) < 1e-10)
                {
                    throw new Exception("矩阵奇异，无法求解");
                }



                //2.4 归一化当前行--让主元变成1
                double pivot = augmented[i][i];  // 当前主元
                for (int j = i; j <= n; j++)
                {
                    augmented[i][j] /= pivot;
                }



                //2.5 消元--让下方行的当前列变成0
                for (int k = i + 1; k < n; k++)
                {  // 处理下方所有行
                    double factor = augmented[k][i];  // 当前列的值（要消掉的数）
                    for (int j = i; j <= n; j++)
                    {    // 对每个元素操作
                        augmented[k][j] -= factor * augmented[i][j];  // 减去主行×系数
                    }
                }
            }

            //3、回代求解
            double[] x = new double[n];
            for(int i = n - 1; i >= 0; i--)
            {
                x[i] = augmented[i][n];
                for(int j = i + 1; j < n; j++)
                {
                    x[i] -= augmented[i][j] * x[j];
                }
            }
            return x;
        }

        #endregion

        #region 曲线拟合结果构造
        public class CurveResult
        {
            public string Direction { get; set; }

            public double A0 { get; set; }
            public double A1 { get; set; }
            public double A2 { get; set; }
            public double B2 { get; set; }
            public double A3 { get; set; }
            public double B3 { get; set; }
            public double A4 { get; set; }
            public double B4 { get; set; }

            public List<TSKB> CleanData { get; set; } = new List<TSKB>();

            //豆包ai，你这PI是怎么在VS中整出来的？
            public override string ToString()
            {
                return $"曲线拟合结果 ({Direction}方向):\n" +
                   $"y(t) = {A0:F6} + {A1:F6}*t + {A2:F6}*cos(2πt) + {B2:F6}*sin(2πt) + " +
                   $"{A3:F6}*cos(4πt) + {B3:F6}*sin(4πt) + {A4:F6}*cos(8πt) + {B4:F6}*sin(8πt)\n" +
                   $"清洗后数据点数量: {CleanData.Count}";
            }
        }

        #endregion
    }
}

四、唠唠叨叨

        时间过得好快，转眼小编就还有十来天就能滚回河北了哈哈。

        今天小编的话痨体质好像下班了，一开始还有有好多想说的，现在却 不知道这26个字母从何按起了哈哈。

        那就祝大家都顺顺利利，加油💪