对于多点求平面，直接的传统方法是三点求平面，通过求解方程组得到平面的法向量和截距。
但仅适用于点集能够恰好拟合到一个平面的情况，如果点集不能恰好拟合到一个平面，则需要使用更复杂的方法来处理，例如使用最小二乘平面拟合或者使用RANSAC算法。
利用所有点到目标平面距离之和最小的条件，筛选出目标平面。
RANSAC是“Random Sample Consensus（随机抽样一致）”的缩写。它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。它是一种不确定的算法，有一定的概率得出一个合理的结果。为了提高得出合理结果的概率必须提高迭代次数。
1、基本思想：
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
有一个模型适用于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
最后，通过估计局内点与模型的错误率来评估模型。
2、对上述步骤，进行简单总结如下：
N：样本点个数，K：求解模型需要的最少的点的个数
随机采样K个点
针对该K个点拟合模型
计算其它点到该拟合模型的距离，小于一定阈值当做内点，统计内点个数
重复M次，选择内点数最多的模型
利用所有的内点重新估计模型（可选）
距离代码
using MathNet.Numerics;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Windows.Media.Media3D;
using MathNet.Numerics.LinearAlgebra.Double;
using System.Windows.Forms.VisualStyles;
namespace 平面拟合
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
}
/// <summary>
/// 点到直线的距离
/// </summary>
/// <param name="point"></param>
/// <param name="plane"></param>
/// <returns></returns>
double PointToPlaneDis(Point3D point, Plane plane)
{
var p = point;
return Math.Abs(plane.Normal.X * p.X + plane.Normal.Y * p.Y + plane.Normal.Z * p.Z + plane.Intercept) /
Math.Sqrt(plane.Normal.X * plane.Normal.X + plane.Normal.Y * plane.Normal.Y + plane.Normal.Z * plane.Normal.Z);
}
/// <summary>
/// 随机抽样法多点拟合平面
/// </summary>
/// <param name="points"></param>
/// <param name="iterations">迭代计算次数一般大于点数量</param>
/// <param name="threshold">筛选条件点到平面的距离</param>
/// <param name="OkPointMin">满足条件的最多点数</param>
/// <param name="OkPointMax">满足条件的最少点数</param>
/// <returns></returns>
public Plane FitPlaneRANSAC(List<Point3D> points, int iterations, double threshold,int OkPointMin, int OkPointMax,int TryCnt=10)
{
// 按照RANSAC算法迭代次数进行循环
int maxInliers = 0;
Plane bestPlane = null;
var step = threshold * 0.1;
int trayCnt = 0;
Retry:
trayCnt++;
List<int> recordIdx = new List<int> { 0, 0, 0 };
List<List<int>> recordIdxList = new List<List<int>>();
// 随机选择三个点构成一个平面
Random rand = new Random();
for (int i = 0; i < iterations; i++)
{
int index1 = rand.Next(points.Count);
int index2 = rand.Next(points.Count);
int index3 = rand.Next(points.Count);
while (index1 == index2 || index1 == index3 || index2 == index3)
{
index1 = rand.Next(points.Count);
index2 = rand.Next(points.Count);
index3 = rand.Next(points.Count);
}
//检测是否已经计算过该平面
foreach (var record in recordIdxList)
{
if (record.Contains(index1) && record.Contains(index2) && record.Contains(index3))
{
continue;
}
}
//记录已经计算的平面
recordIdxList.Add(new List<int> { index1, index2, index3 });
Point3D p1 = points[index1];
Point3D p2 = points[index2];
Point3D p3 = points[index3];
Plane plane = new Plane(p1, p2, p3);
// 计算与该平面距离小于阈值的点数
int inliers = 0;
foreach (Point3D p in points)
{
double distance = PointToPlaneDis(p, plane);
if (distance <= threshold)
{
inliers++;
}
}
// 如果当前平面的支持点数比之前的最好平面要多，则更新最好平面
if (inliers > maxInliers)
{
maxInliers = inliers;
bestPlane = plane;
}
}
if(trayCnt> TryCnt)
{
//
return bestPlane;
}
if(maxInliers>OkPointMax)
{
threshold -= step;//自动调整条件范围
maxInliers = 0;
goto Retry;
}
else if (maxInliers < OkPointMin)
{
threshold += step;//自动调整条件范围
maxInliers = 0;
goto Retry;
}else
{
return bestPlane;
}
return bestPlane;
}
/// <summary>
/// 使用到所有点到目标平面的距离之和最小筛选平面
/// </summary>
/// <param name="points"></param>
/// <param name="iterations"></param>
/// <returns></returns>
public Plane FitPlaneRANSACByAllHight(List<Point3D> points, int iterations)
{
// 按照RANSAC算法迭代次数进行循环
double maxInliers = 9999;
//防止距离和超出double的最大值，使用两个数记录
double maxInliers2 = 9999;
Plane bestPlane = null;
List<List<int>> recordIdxList = new List<List<int>>();
// 随机选择三个点构成一个平面
Random rand = new Random();
for (int i = 0; i < iterations; i++)
{
int index1 = rand.Next(points.Count);
int index2 = rand.Next(points.Count);
int index3 = rand.Next(points.Count);
while (index1 == index2 || index1 == index3 || index2 == index3)
{
index1 = rand.Next(points.Count);
index2 = rand.Next(points.Count);
index3 = rand.Next(points.Count);
}
//检测是否已经计算过该平面
foreach (var record in recordIdxList)
{
if (record.Contains(index1) && record.Contains(index2) && record.Contains(index3))
{
continue;
}
}
//记录已经计算的平面
recordIdxList.Add(new List<int> { index1, index2, index3 });
Point3D p1 = points[index1];
Point3D p2 = points[index2];
Point3D p3 = points[index3];
Plane plane = new Plane(p1, p2, p3);
// 计算与该平面距离的和
double inliers = 0;
double inliers2 = 0;
foreach (Point3D p in points)
{
double distance = PointToPlaneDis(p, plane);
if (inliers < double.MaxValue - distance - 1)
{
inliers += distance;
}
else
{
if (inliers2 < double.MaxValue - distance - 1)
{
inliers2 += distance;
}
else
return null;
}
}
if (inliers2 ==0)
{
// 如果当前平面的支持点数比之前的最好平面要好，则更新最好平面
if (inliers < maxInliers)
{
maxInliers = inliers;
bestPlane = plane;
}
}else
{
if (inliers2 +(inliers- double.MaxValue) < maxInliers2+ (maxInliers - double.MaxValue))
{
maxInliers2 = inliers2;
bestPlane = plane;
}
}
}
return bestPlane;
}
/// <summary>
/// 平面拟合, 返回拟合度,范围[0,1],越接近1越好
/// </summary>
/// <param name="pointList">输入三维点列表</param>
/// <param name="pixelSize">1像素=?mm, 用于输入的点坐标是像素时转换为mm</param>
/// <param name="param">输出平面参数</param>
/// <param name="titl">输出平面角度XY,高度Z</param>
/// <returns></returns>
private double PlaneFit(List<Point3D> pointList, double pixelSize, out double[] param, out double[] titl)
{
param = new double[] { 0, 0, 0 };
titl = new double[] { 0, 0, 0 };
if (pointList.Count == 0) return -1;
for (var i = 0; i < pointList.Count; i++)
{
var pt = pointList[i];
pt.X *= pixelSize;
pt.Y *= pixelSize;
pointList[i] = pt;
}
var a = new double[3, 3];
var b = new double[3];
double[] c = { 0, 0, 0 };
a[0, 0] = pointList.Sum(pt => pt.X * pt.X);
a[0, 1] = pointList.Sum(pt => pt.X * pt.Y);
a[0, 2] = pointList.Sum(pt => pt.X);
a[1, 0] = pointList.Sum(pt => pt.Y * pt.X);
a[1, 1] = pointList.Sum(pt => pt.Y * pt.Y);
a[1, 2] = pointList.Sum(pt => pt.Y);
a[2, 0] = pointList.Sum(pt => pt.X);
a[2, 1] = pointList.Sum(pt => pt.Y);
a[2, 2] = pointList.Count;
b[0] = pointList.Sum(pt => pt.X * pt.Z);
b[1] = pointList.Sum(pt => pt.Y * pt.Z);
b[2] = pointList.Sum(pt => pt.Z);
var matrixA = DenseMatrix.OfArray(a);
var matrixB = DenseMatrix.OfColumnArrays(b);
var matrixC = matrixA.Solve(matrixB); //求解AC=B的线性方程组,input为右矩阵B,返回求解结果矩阵C
c = matrixC.ToColumnMajorArray();
param = c;
//拟合误差
var listOld = new List<double>();
var listNew = new List<double>();
for (var i = 0; i < pointList.Count; i++)
{
var temp = c[0] * pointList[i].X + c[1] * pointList[i].Y + c[2];
listOld.Add(pointList[i].Z);
listNew.Add(temp);
}
var cnx = pointList.Sum(pt => pt.X) / pointList.Count;
var cny = pointList.Sum(pt => pt.Y) / pointList.Count;
var tz = param[0] * cnx + param[1] * cny + param[2];
var tx = Math.Atan(param[0]) * 180 / Math.PI;
var ty = Math.Atan(param[1]) * 180 / Math.PI;
titl[0] = tx;
titl[1] = ty;
titl[2] = tz;
var rs = GoodnessOfFit.RSquared(listOld, listNew);
return rs;
}
public bool CalcPlaneTilt(out string outErrMsg, List<Point3D> pointList, out Point3D outTilt)
{
var res = true;
outErrMsg = "";
outTilt = new Point3D();
var pointFs = new List<Point3D>();
var ff = new Point3D(1000, 1000, 1000);
if (pointList.Count < 3)
{
MessageBox.Show( "计算tilt点数不足3个不能计算平面");
return false;
}
foreach (var pt in pointList)
pointFs.Add(new Point3D( pt.X*1000, pt.Y* 1000, pt.Z * 1000));
//平面拟合
double[] PlaneParam;
double[] PlaneTilt;
var rPlaneSquaredLimit = 0.0000001;
var rSquared = PlaneFit(pointFs, 1, out PlaneParam, out PlaneTilt); //point已经是mm,所以 pixelSize取1
if (rSquared > rPlaneSquaredLimit || double.IsNaN(rSquared))
{
//Tilt值 XYZ, Z是清晰位置高度
outTilt.X = Math.Round(PlaneTilt[0], 3);
outTilt.Y = Math.Round(PlaneTilt[1], 3);
outTilt.Z = Math.Round(PlaneTilt[2] / 1000, 4);
return true;
}
else
{
return false;
}
return res;
}
private async void btnPointCnt_Click(object sender, EventArgs e)
{
Plane bestPlane = null;
Point3D tilt = new Point3D();
Task tk = Task.Factory.StartNew(() =>
{
List<Point3D> AllPoints = new List<Point3D>();
int pointcnt = 100;
Random rand = new Random();
//假设平面5x+4y+z+1=0;生成距离平面距离10以内的300个点
//使用这300个点，进行随机平面拟合匹配验证结果和目标平面对比
while (pointcnt > 0)
{
double X = rand.Next(1, 100);
double Y = rand.Next(1, 100);
var mSqrt = Math.Sqrt(5 * 5 + 4 * 4 + 1);
//生成随机距离
double randDisZ = (rand.Next(-100, 100)) / 10;
//利用距离反求出点的坐标Z
double Z = randDisZ * mSqrt - 5 * X - 4 * Y - 1;
AllPoints.Add(new Point3D(X, Y, Z));
pointcnt--;
}
pointcnt = AllPoints.Count;
bestPlane = FitPlaneRANSAC(AllPoints, 900, 10, pointcnt / 2, pointcnt - 20);
});
await tk;
Thread.Sleep(200);
var p = bestPlane.Normal;
var D = bestPlane.Intercept;
//把Ax+By+Cz+D=0转化两边同时除以C，消去Z的系数
MessageBox.Show($"求出直线{p.X / p.Z}X+{p.Y / p.Z}Y + Z+{D / p.Z}=0");
}
private async void btnPointDisAll_Click(object sender, EventArgs e)
{
Plane bestPlane = null;
Point3D tilt = new Point3D();
Task tk = Task.Factory.StartNew(() => {
List<Point3D> AllPoints = new List<Point3D>() {
new Point3D(x: 22.892,y:2.789,z:2.491),
new Point3D(x: 23.460,y:3.789,z:2.622),
new Point3D(x: 25.192,y:5.089,z:2.358),
new Point3D(x: 26.392,y:4.867,z:2.290),
new Point3D(x: 27.098,y:3.889,z:2.077),
new Point3D(x: 27.192,y:2.789,z:2.283),
new Point3D(x: 26.924,y:1.789,z:2.316),
new Point3D(x: 26.242,y:0.970,z:2.327),
new Point3D(x: 25.192,y:0.989,z:1.981),
new Point3D(x: 24.142,y:0.970,z:2.309),
new Point3D(x: 23.114,y:1.589,z:2.331),
};
List<double> zlist = new List<double>() {
2.4768 ,
2.6202 ,
2.3562 ,
2.2881 ,
2.078 ,
2.2833 ,
2.315 ,
2.326 ,
1.9901 ,
2.3084 ,
2.3301 };
int i = 0;
//while (i <= AllPoints.Count - 1)
//{
// var pp = new Point3D(AllPoints[i].X, AllPoints[i].Y, zlist[i]);
// AllPoints[i] = pp;
// i++;
//}
bestPlane = FitPlaneRANSACByAllHight(AllPoints, 19900);
CalcPlaneTilt(out var msg, bestPlane.pointXyzList, out tilt);
});
await tk;
Thread.Sleep(200);
var p = bestPlane.Normal;
var D = bestPlane.Intercept;
//显示和Z轴的夹角（X方向和Y方向，以及Z平均值）
MessageBox.Show($"求出tilt:X{tilt.X }+Y{tilt.Y } + Z+{tilt.Z}");
}
}
public class Plane
{
public Vector3D Normal { get; set; }
public double Intercept { get; set; }
public List<Point3D> pointXyzList
{
get
{
if (Normal.Z != 0 && Normal.X != 0 && Normal.Y != 0)
return new List<Point3D>(){new Point3D() { X = 0,Y = 0,Z = -Intercept / Normal.Z},
new Point3D() { Z = 0,Y = 0,X = -Intercept / Normal.X},
new Point3D() { Z = 0,X = 0,Y = -Intercept / Normal.Y} };
else
{
return new List<Point3D>() { new Point3D() { X = 0, Y = 0, Z = 0 } };
}
}
}
public Plane(Point3D p1, Point3D p2, Point3D p3)
{
// 计算法向量
Vector3D v1 = new Vector3D(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
Vector3D v2 = new Vector3D(p3.X - p1.X, p3.Y - p1.Y, p3.Z - p1.Z);
Normal = Vector3D.CrossProduct(v1, v2);
Normal.Normalize();
// 计算截距
Intercept = -(Normal.X * p1.X + Normal.Y * p1.Y + Normal.Z * p1.Z);
}
}
}
该代码首先通过循环执行一定次数的迭代，每次迭代随机选择三个点构成一个平面，计算与该平面距离小于阈值的点数，然后选取支持点数最多的平面作为最好平面。在实际应用中，需要根据具体情况调整迭代次数和阈值的取值。
扫一扫
422
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
