private Plane checkForSimilarPlanes(Plane current, IEnumerable <Plane> usedPlanes)
{
var similarPlanes = usedPlanes.Where(plane => RansacPlaneFinder.Similar(plane, current, this.PointCloud.GroundPoint));
if (similarPlanes.Any())
{
return(similarPlanes.First());
}
else
{
return(current);
}
}
public void Classify(IEnumerable <int> indicesParam)
{
if (this.PointCloud.Normals == null || this.PointCloud.Normals.Length == 0)
{
this.PointCloud.EstimateNormals();
}
List <int> indices;
if (indicesParam == null)
{
indices = Enumerable.Range(0, this.PointCloud.Points.Length).ToList();
}
else
{
indices = indicesParam.ToList();
}
while (indices.Count() > RansacPlaneFinder.randomSampleCount)
{
indices.RemoveAt(Random.Range(0, indices.Count));
}
var planes = indices.Select(i => new Plane(this.PointCloud.Normals[i], this.PointCloud.Points[i])).ToList();
this.PlanesWithScore = planes.Select(plane => new Tuple <Plane, float>(plane, this.PointCloud.GetScore(plane)))
.OrderByDescending(tuple => tuple.Value2).ToList();
for (int i = 0; i < this.PlanesWithScore.Count; i++)
{
var plane = this.PlanesWithScore.ElementAt(i).Value1;
for (int j = i + 1; j < this.PlanesWithScore.Count; j++)
{
if (RansacPlaneFinder.Similar(plane, this.PlanesWithScore[j].Value1, this.PointCloud.GroundPoint))
{
this.PlanesWithScore.RemoveAt(j);
j--;
}
}
}
}
public void CreateMeshCutoff(bool createAttachments)
{
this.CheckForPlanes();
Timekeeping.CompleteTask("Find planes");
float bestScore = -1.0f;
IEnumerable <Triangle> bestMesh = null;
IEnumerable <Plane> bestPlanes = null;
foreach (var selectedPlanes in this.Planes.Take(5).Subsets())
{
var currentMesh = this.createFromPlanes(selectedPlanes);
var currentScore = this.GetScore(currentMesh);
if (currentScore > bestScore)
{
bestScore = currentScore;
bestMesh = currentMesh;
bestPlanes = selectedPlanes;
}
}
var resultMesh = bestMesh.ToList();
var usedPlanes = bestPlanes.ToList();
Timekeeping.CompleteTask("Convex roof");
if (!createAttachments)
{
this.Triangles = resultMesh;
this.CleanMesh = false;
return;
}
var planeFinder = new RansacPlaneFinder(this.PointCloud);
int attachmentCount = 0;
foreach (var plane in bestPlanes)
{
var indices = new List <int>();
for (int i = 0; i < this.PointCloud.Points.Length; i++)
{
if (plane.GetDistanceToPoint(this.PointCloud.Points[i]) > HoughPlaneFinder.MaxDistance * 0.5f)
{
indices.Add(i);
}
}
while (true)
{
planeFinder.Classify(indices);
planeFinder.RemoveGroundPlanesAndVerticalPlanes();
var outsidePlanes = planeFinder.PlanesWithScore.Where(tuple => tuple.Value2 > 2.0f).Select(tuple => tuple.Value1).Where(newPlane => !RansacPlaneFinder.Similar(plane, newPlane, this.PointCloud.GroundPoint));
outsidePlanes = outsidePlanes.Select(p => this.checkForSimilarPlanes(p, usedPlanes));
if (outsidePlanes.Count() == 0)
{
break;
}
bestScore = 0.0f;
IEnumerable <Plane> planesInAttachment = null;
foreach (var selectedPlanes in outsidePlanes.Take(5).Subsets())
{
var currentMesh = this.createFromPlanes(selectedPlanes);
currentMesh = Triangle.CutMesh(currentMesh, plane, true);
float pointDensity = currentMesh.Sum(t => t.GetPointCount(this.PointCloud, indices)) / currentMesh.Sum(t => t.GetArea());
if (pointDensity < 3.0f)
{
continue;
}
var currentScore = currentMesh.Sum(triangle => triangle.GetScore(this.PointCloud, indices));
if (currentScore > bestScore)
{
bestScore = currentScore;
bestMesh = currentMesh;
planesInAttachment = selectedPlanes;
}
}
if (bestScore > 2.0f)
{
resultMesh.AddRange(bestMesh);
attachmentCount++;
usedPlanes = usedPlanes.Union(planesInAttachment).ToList();
}
else
{
break;
}
indices = indices.Where(i => !bestMesh.Any(triangle => triangle.Contains(this.PointCloud.Points[i]))).ToList();
}
}
Timekeeping.CompleteTask("Find attachments");
if (attachmentCount == 0)
{
this.CleanMesh = false;
}
this.Triangles = resultMesh;
}
