public Line3 Estimate(List <MyVector3> points)
{
int iter = 200;
if (points.Count == 0)
{
return(null);
}
Random rd = new Random();
MyVector3 A = new MyVector3();
MyVector3 B = new MyVector3();
int maxpointinline = int.MinValue;
for (int i = 0; i < iter; i++)
{
A = points[rd.Next(points.Count)];
B = points[rd.Next(points.Count)];
if (A == B)
{
continue; //if can't generate line
}
Line3 testline = new Line3(A, (B - A).Normalize());
List <MyVector3> tempinliers = new List <MyVector3>();
int inlierscount = 0;
for (int j = 0; j < points.Count; j++)
{
if (points[j] != A && points[j] != B)
{
if (testline.DistanceToLine(points[j]) < thres)
{
tempinliers.Add(points[j]);
inlierscount++;
}
}
}
if (inlierscount > maxpointinline)
{
maxpointinline = inlierscount;
this.bestline = testline.Copy();
this.inliers.Clear();
foreach (MyVector3 p in tempinliers)
{
this.inliers.Add(p);
}
}
if (inlierscount >= probability * points.Count)
{
break;
}
}
if (this.inliers.Count != 0)
{
double mint = double.MaxValue;
double maxt = double.MinValue;
foreach (MyVector3 p in this.inliers)
{
double t = this.bestline.ComputeT(p);
if (t > maxt)
{
maxt = t;
}
if (t < mint)
{
mint = t;
}
}
bestline.SetPoints(mint, maxt);
}
if (bestline != null && bestline.startpoint.x != double.NaN && bestline.startpoint.y != double.NaN && bestline.startpoint.z != double.NaN &&
bestline.endpoint.x != double.NaN && bestline.endpoint.y != double.NaN && bestline.endpoint.z != double.NaN &&
(!bestline.startpoint.IsNull() && !bestline.endpoint.IsNull()))
{
return(bestline);
}
else
{
return(null);
}
}