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);
    }
Exemple #3
0
    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;
    }
Exemple #4
0
    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;
    }