private void houghTransform()
{
houghSpace = new float[ranges[0], ranges[1], ranges[2]];
Timekeeping.CompleteTask("Create Hough Space");
for (int i0 = 0; i0 < ranges[0]; i0++)
{
for (int i1 = 0; i1 < ranges[1]; i1++)
{
Plane plane = this.getHoughPlane(i0, i1);
for (int i = 0; i < this.PointCloud.Points.Length; i++)
{
float distance = -plane.GetDistanceToPoint(this.PointCloud.Points[i] + Vector3.down * this.PointCloud.GroundPoint.y);
int start = Mathf.FloorToInt(map(this.min[2], this.max[2], 0, this.ranges[2], distance - HoughPlaneFinder.MaxDistance));
int end = Mathf.CeilToInt(map(this.min[2], this.max[2], 0, this.ranges[2], distance + HoughPlaneFinder.MaxDistance));
if ((start >= 0 && start < ranges[2]) || (end >= 0 && end < ranges[2]))
{
for (int i2 = limit(0, ranges[2] - 1, start); i2 <= limit(0, ranges[2] - 1, end); i2++)
{
float relativeDistance = Mathf.Abs(map(0, ranges[2], min[2], max[2], i2) - distance) / HoughPlaneFinder.MaxDistance;
houghSpace[i0, i1, i2] += HoughPlaneFinder.getScore(relativeDistance);
}
}
}
}
}
Timekeeping.CompleteTask("Find maxima");
this.PlanesWithScore = new List <Tuple <Plane, float> >();
float maxScore = 0;
for (int i0 = 0; i0 < ranges[0]; i0++)
{
for (int i1 = 0; i1 < ranges[1]; i1++)
{
for (int i2 = 0; i2 < ranges[2]; i2++)
{
if (houghSpace[i0, i1, i2] > minScoreRelative * this.PointCloud.Points.Length && this.isLocalMaximum(i0, i1, i2, 3))
{
Plane plane = this.getHoughPlane(i0, i1, i2);
this.PlanesWithScore.Add(new Tuple <Plane, float>(plane, houghSpace[i0, i1, i2]));
}
if (houghSpace[i0, i1, i2] > maxScore)
{
maxScore = houghSpace[i0, i1, i2];
}
}
}
}
Timekeeping.CompleteTask("Find planes");
this.printHoughArray();
}
public Mesh GetMesh()
{
if (this.mesh == null)
{
if (this.CleanMesh)
{
var cleanedTriangles = MeshCleaner.CleanMesh(this.Triangles);
Timekeeping.CompleteTask("Clean mesh");
this.mesh = Triangle.CreateMesh(cleanedTriangles, true);
}
else
{
this.mesh = Triangle.CreateMesh(this.Triangles, true);
}
}
return(this.mesh);
}
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;
}
public void CreateMesh()
{
this.CheckForPlanes();
Timekeeping.Reset();
var result = new List <Triangle>();
var remainingPointIndices = Enumerable.Range(0, this.PointCloud.Points.Length).ToList();
foreach (var plane in this.Planes.Take(8))
{
var onPlane = this.PointCloud.Points.Where(p => Mathf.Abs(plane.GetDistanceToPoint(p)) < HoughPlaneFinder.MaxDistance);
Timekeeping.CompleteTask("Select points");
var planeCoordinates = new PlaneCoordinates(plane);
var planePoints = onPlane.Select(p => planeCoordinates.ToPlane(p)).ToList();
var triangles = PointMeshCreator.Triangulate(planePoints).ToArray();
Timekeeping.CompleteTask("Triangulate");
const float maxDistance = 1.5f;
var edges = new List <Tuple <int, int> >();
for (int i = 0; i < triangles.Count(); i += 3)
{
float dst1 = (planePoints[triangles[i]] - planePoints[triangles[i + 1]]).magnitude;
float dst2 = (planePoints[triangles[i + 1]] - planePoints[triangles[i + 2]]).magnitude;
float dst3 = (planePoints[triangles[i + 2]] - planePoints[triangles[i]]).magnitude;
if (dst1 < maxDistance && dst2 < maxDistance && dst2 < maxDistance)
{
edges.Add(new Tuple <int, int>(triangles[i], triangles[i + 1]));
edges.Add(new Tuple <int, int>(triangles[i + 1], triangles[i + 2]));
edges.Add(new Tuple <int, int>(triangles[i + 2], triangles[i]));
}
}
Timekeeping.CompleteTask("Discard bad triangles");
var neighbours = new Dictionary <int, HashSet <int> >();
foreach (var edge in edges)
{
if (!neighbours.ContainsKey(edge.Value1))
{
neighbours[edge.Value1] = new HashSet <int>();
}
neighbours[edge.Value1].Add(edge.Value2);
if (!neighbours.ContainsKey(edge.Value2))
{
neighbours[edge.Value2] = new HashSet <int>();
}
neighbours[edge.Value2].Add(edge.Value1);
}
var outsideEdges = new List <Tuple <int, int> >();
foreach (var edge in edges)
{
var rayBase = planePoints[edge.Value1];
var rayDirection = planePoints[edge.Value2] - planePoints[edge.Value1];
var orthogonal = new Vector2(-rayDirection.y, rayDirection.x);
var candidates = neighbours[edge.Value1].Where(i => neighbours[edge.Value2].Contains(i)).Select(i => planePoints[i]);
bool left = false;
bool right = false;
foreach (var point in candidates)
{
if (Vector2.Angle(point - rayBase, orthogonal) < 90.0f)
{
left = true;
}
else
{
right = true;
}
if (left && right)
{
break;
}
}
if (!left || !right)
{
outsideEdges.Add(edge);
}
}
Timekeeping.CompleteTask("Find outside edges");
var nextOutsideEdge = new Dictionary <int, HashSet <Tuple <int, int> > >();
foreach (var edge in outsideEdges)
{
if (!nextOutsideEdge.ContainsKey(edge.Value1))
{
nextOutsideEdge[edge.Value1] = new HashSet <Tuple <int, int> >();
}
nextOutsideEdge[edge.Value1].Add(edge);
if (!nextOutsideEdge.ContainsKey(edge.Value2))
{
nextOutsideEdge[edge.Value2] = new HashSet <Tuple <int, int> >();
}
nextOutsideEdge[edge.Value2].Add(edge);
}
var visitedEdges = new HashSet <Tuple <int, int> >();
var resultPoints = new List <Vector3>();
foreach (var edge in outsideEdges)
{
if (visitedEdges.Contains(edge))
{
continue;
}
var polygonIndices = new List <int>();
var currentPoint = edge.Value1;
polygonIndices.Add(currentPoint);
visitedEdges.Add(edge);
while (true)
{
var nextEdge = nextOutsideEdge[currentPoint].FirstOrDefault(e => !visitedEdges.Contains(e));
if (nextEdge == null)
{
break;
}
visitedEdges.Add(nextEdge);
if (nextEdge.Value1 == currentPoint)
{
currentPoint = nextEdge.Value2;
}
else
{
currentPoint = nextEdge.Value1;
}
polygonIndices.Add(currentPoint);
}
if (polygonIndices.Count > 10)
{
IEnumerable <Vector2> polygon = this.simplifyPolygon(polygonIndices.Select(i => planePoints[i]), 2.0f, 20.0f, 160.0f, 5);
polygon = this.snapPoints(polygon, planeCoordinates, resultPoints, 2f);
resultPoints.AddRange(polygon.Select(p => planeCoordinates.ToWorld(p)));
var meshTriangles = this.polygonToTriangle(polygon, planeCoordinates);
var score = meshTriangles.Sum(triangle => triangle.GetPointCount(this.PointCloud, remainingPointIndices));
Debug.Log(score);
if (score > 60)
{
result.AddRange(meshTriangles);
remainingPointIndices = remainingPointIndices.Where(i => !meshTriangles.Any(triangle => triangle.Contains(this.PointCloud.Points[i]))).ToList();
}
}
}
Timekeeping.CompleteTask("Find polygons");
}
this.Triangles = result;
}