Esempio n. 1
0
    public void GeneratePointCloud(bool realtimeExport)
    {
        HashSet <MeshFilter> meshFilterDatabase = new HashSet <MeshFilter>();
        var pointCloudAreas      = GetComponentsInChildren <PointCloudArea>();
        int totalCount           = 0;
        int batchExportIndex     = 0;
        int lastBatchExportIndex = batchExportIndex;
        List <PointCloudVertex> pointCloudVertices = null;

        if (gameobjects.Count > 0)
        {
            GameObject lastGo = gameobjects[0];
            for (int ind = 0; ind < gameobjects.Count; ind++)
            {
                var go = gameobjects[ind];

                if (go == null)
                {
                    continue;
                }

                lastBatchExportIndex = batchExportIndex;
                batchExportIndex     = 0;

                if (filenamePerObject && !realtimeExport)
                {
                    pointCloudPool.Add(go, new List <PointCloudVertex>());
                    pointCloudVertices = pointCloudPool[go];
                }
                else
                {
                    if (!pointCloudPool.ContainsKey(gameObject))
                    {
                        pointCloudPool.Add(gameObject, new List <PointCloudVertex>());
                    }
                    pointCloudVertices = pointCloudPool[gameObject];
                }

                var meshFilters = go.GetComponentsInChildren <MeshFilter>();

                foreach (var mFilter in meshFilters)
                {
                    var theMesh  = mFilter.sharedMesh;
                    var renderer = mFilter.gameObject.GetComponent <Renderer>();
                    if (theMesh == null || renderer == null)
                    {
                        continue;
                    }

                    if (!meshFilterDatabase.Contains(mFilter))
                    {
                        mFilter.gameObject.isStatic = false;

                        for (int i = 0; i < theMesh.subMeshCount; i++)
                        {
                            var triangles = new List <int>();
                            theMesh.GetTriangles(triangles, i);
                            for (int j = 0; j < triangles.Count; j += 3)
                            {
                                var A = theMesh.vertices[triangles[j]];
                                var B = theMesh.vertices[triangles[j + 1]];
                                var C = theMesh.vertices[triangles[j + 2]];

                                var A_wld = mFilter.transform.TransformPoint(A);
                                var B_wld = mFilter.transform.TransformPoint(B);
                                var C_wld = mFilter.transform.TransformPoint(C);

                                var center_wld = (A_wld + B_wld + C_wld) / 3.0f;

                                float pointAmount = GetTriangleArea(A_wld, B_wld, C_wld) * density;
                                float extraAmount = .0f;

                                int   totalAreas        = 0;
                                float totalExtraScalers = .0f;
                                foreach (var pcArea in pointCloudAreas)
                                {
                                    if (pcArea.Contains(center_wld))
                                    {
                                        totalExtraScalers += pcArea.densityScaler;
                                        ++totalAreas;
                                    }
                                }
                                if (totalAreas > 0)
                                {
                                    extraAmount = pointAmount * (totalExtraScalers / totalAreas) - pointAmount;
                                }

                                List <Vector3> points = new List <Vector3>();
                                if (Random.value < (pointAmount % 1.0f))
                                {
                                    ++pointAmount;
                                }
                                for (int k = 0; k < System.Math.Round(pointAmount); k++)
                                {
                                    points.Add(GetRandomPointFromTriangle(A_wld, B_wld, C_wld));
                                }

                                if (Random.value < (extraAmount % 1.0f))
                                {
                                    ++extraAmount;
                                }
                                for (int k = 0; k < System.Math.Round(extraAmount); k++)
                                {
                                    var  p    = GetRandomPointFromTriangle(A_wld, B_wld, C_wld);
                                    bool keep = false;
                                    foreach (var pcArea in pointCloudAreas)
                                    {
                                        if (pcArea.Contains(p))
                                        {
                                            keep = true;
                                        }
                                    }
                                    if (keep)
                                    {
                                        points.Add(p);
                                    }
                                }

                                foreach (var p in points)
                                {
                                    if (boundShape == null || boundShape.Contains(p))
                                    {
                                        Material mat = mFilter.gameObject.GetComponent <Renderer>().sharedMaterials[i];
                                        Vector2  uv  = GetInterpolatedPointFromTriangle(p, A_wld, B_wld, C_wld, theMesh.uv[triangles[j]], theMesh.uv[triangles[j + 1]], theMesh.uv[triangles[j + 2]]);

                                        Texture2D tex = mat.mainTexture as Texture2D;
                                        Color     c   = Color.black;
                                        if (tex != null)
                                        {
                                            try
                                            {
                                                c = tex.GetPixelBilinear(uv.x, uv.y);
                                            }
                                            catch (System.Exception)
                                            {
                                                Debug.Log("Texture GetPixel error:  " + tex.name);
                                                return;
                                            }
                                        }

                                        pointCloudVertices.Add(new PointCloudVertex
                                        {
                                            position = p,
                                            // normal = GetInterpolatedPointFromTriangle(p, A_wld, B_wld, C_wld, mFilter.transform.TransformVector(theMesh.normals[triangles[j]]), mFilter.transform.TransformVector(theMesh.normals[triangles[j + 1]]), mFilter.transform.TransformVector(theMesh.normals[triangles[j + 2]])),
                                            uv       = uv,
                                            color    = c,
                                            material = mat,
                                        });
                                        ++totalCount;

                                        if (realtimeExport)
                                        {
                                            if (filenamePerObject)
                                            {
                                                if (go != lastGo)
                                                {
                                                    RealtimeExport(pointCloudVertices, lastGo.name, lastBatchExportIndex);
                                                    lastGo = go;
                                                    pointCloudVertices.Clear();
                                                }
                                                else if (pointCloudVertices.Count >= batchSize)
                                                {
                                                    RealtimeExport(pointCloudVertices, go.name, batchExportIndex++);
                                                    pointCloudVertices.Clear();
                                                }
                                            }
                                            else
                                            {
                                                if (pointCloudVertices.Count >= batchSize)
                                                {
                                                    RealtimeExport(pointCloudVertices, "", batchExportIndex++);
                                                    pointCloudVertices.Clear();
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }

                        mFilter.gameObject.isStatic = true;

                        meshFilterDatabase.Add(mFilter);
                    }
                }
            }

            if (realtimeExport && pointCloudVertices.Count > 0)
            {
                if (filenamePerObject)
                {
                    RealtimeExport(pointCloudVertices, gameobjects[gameobjects.Count - 1].name, batchExportIndex++);
                }
                else
                {
                    RealtimeExport(pointCloudVertices, "", batchExportIndex++);
                }
            }
        }

        Debug.Log("Total Point Cloud Vertices Count: " + totalCount);
    }
Esempio n. 2
0
    private void FixedUpdate()
    {
        // Do nothing, if the simulator is paused.
        if (!isPlaying)
        {
            return;
        }

        // Check if number of steps is greater than possible calculations by unity.
        float numberOfStepsNeededInOneLap = 360 / Mathf.Abs(rotationAnglePerStep);
        float numberOfStepsPossible       = 1 / Time.fixedDeltaTime / rotationSpeedHz;

        // Check if it is time to step. Example: 2hz = 2 rotations in a second.
        var neededInterval = 1f / (numberOfStepsNeededInOneLap * rotationSpeedHz);
        var diff           = Time.fixedTime - lastUpdate;

        if (diff > neededInterval)
        {
            int precalculateIterations = (int)(diff / neededInterval);
            for (int i = 0; i < precalculateIterations; i++)
            {
                // Perform rotation.
                transform.Rotate(0, rotationAnglePerStep, 0, Space.Self);

                // Execute lasers.
                for (int x = 0; x < lasers.Count; x++)
                {
                    RaycastHit hit      = lasers[x].ShootRay(LidarBitmask);
                    float      distance = hit.distance;
                    if (distance != 0 && (filterShape == null || !filterShape.Contains(hit.point))) // Didn't hit anything or in filter shape, don't add to list.
                    {
                        //float verticalAngle = lasers[x].GetVerticalAngle();
                        Color    c;
                        Renderer rend = hit.transform.GetComponent <Renderer>();
                        if (rend != null && (hit.collider as MeshCollider) != null && rend.sharedMaterial != null && rend.sharedMaterial.mainTexture != null)
                        {
                            Texture2D tex     = rend.sharedMaterial.mainTexture as Texture2D; //Can be improved later dealing with multiple share materials
                            Vector2   pixelUV = hit.textureCoord;
                            c = tex.GetPixelBilinear(pixelUV.x, pixelUV.y);
                        }
                        else
                        {
                            c = Color.black;
                        }

                        pointCloud.Add(new VelodynePointCloudVertex()
                        {
                            position   = hit.point,
                            ringNumber = (System.UInt16)x,
                            distance   = distance,
                            color      = c,
                        });
                    }
                }

                horizontalAngle += rotationAnglePerStep; // Keep track of our current rotation.
                if (horizontalAngle >= 360)
                {
                    horizontalAngle -= 360;
                    lastLapTime      = Time.fixedTime;
                    publishTimeStamp = Time.fixedTime;
                    SendPointCloud(pointCloud);
                    pointCloud.Clear();
                }

                // Update current execution time.
                lastUpdate += neededInterval;
            }
        }
    }
Esempio n. 3
0
    private void FixedUpdate()
    {
        // Do nothing, if the simulator is paused.
        if (!isPlaying)
        {
            return;
        }

        // Check if number of steps is greater than possible calculations by unity.
        float numberOfStepsNeededInOneLap = 360 / Mathf.Abs(rotationAnglePerStep);
        float numberOfStepsPossible       = 1 / Time.fixedDeltaTime / 5;
        float precalculateIterations      = 1;

        // Check if we need to precalculate steps.
        if (numberOfStepsNeededInOneLap > numberOfStepsPossible)
        {
            precalculateIterations = (int)(numberOfStepsNeededInOneLap / numberOfStepsPossible);
            if (360 % precalculateIterations != 0)
            {
                precalculateIterations += 360 % precalculateIterations;
            }
        }

        // Check if it is time to step. Example: 2hz = 2 rotations in a second.
        if (Time.fixedTime - lastUpdate > (1f / (numberOfStepsNeededInOneLap) / rotationSpeedHz) * precalculateIterations)
        {
            // Update current execution time.
            lastUpdate = Time.fixedTime;

            for (int i = 0; i < precalculateIterations; i++)
            {
                // Perform rotation.
                transform.Rotate(0, rotationAnglePerStep, 0, Space.Self);
                horizontalAngle += rotationAnglePerStep; // Keep track of our current rotation.
                if (horizontalAngle >= 360)
                {
                    horizontalAngle -= 360;
                    lastLapTime      = Time.fixedTime;
                    publishTimeStamp = Time.fixedTime;
                    SendPointCloud(pointCloud);
                    pointCloud.Clear();
                }

                // Execute lasers.
                for (int x = 0; x < lasers.Count; x++)
                {
                    RaycastHit hit      = lasers[x].ShootRay();
                    float      distance = hit.distance;
                    if (distance != 0 && (filterShape == null || !filterShape.Contains(hit.point))) // Didn't hit anything or in filter shape, don't add to list.
                    {
                        float verticalAngle = lasers[x].GetVerticalAngle();
                        var   pcv           = new VelodynePointCloudVertex();
                        pcv.position   = hit.point;
                        pcv.ringNumber = (System.UInt16)x;
                        pcv.distance   = distance;
                        pointCloud.Add(pcv);
                    }
                }
            }
        }
    }