private void generatePCDOperations()
        {
            Button cameraButton = this.transform.Find("Panel").Find("ButtonCameraMode").GetComponent<Button>();

            if (!hasLaserScanner)
            {
                cameraMode = CameraMode.None;
                alert.closeAlertDialog();
                cameraButton = this.transform.Find("Panel").Find("ButtonCameraMode").GetComponent<Button>();
                cameraButton.interactable = true;

                if (carController != null)
                {
                    carController.wheelColliders[0].brakeTorque = 0;
                    carController.wheelColliders[1].brakeTorque = 0;
                }
                controllerObject.SetActive(true);
                return;
            }

            System.Diagnostics.Stopwatch stopwatch = new System.Diagnostics.Stopwatch();
            stopwatch.Start();

            //This is the Game Object contains transform of the camera Van
            liDAROrigin = new GameObject();

            //This is the Game Object contains transform information of the LiDAR device wrt Camera Van
            GameObject liDAR = new GameObject();
            liDAR.transform.position = laserScanner.position;
            Quaternion liDARquat = new Quaternion();
            liDARquat.eulerAngles = laserScanner.rotation;
            liDAR.transform.rotation = liDARquat;

            //liDAR and Camera Van is Linked
            liDAR.transform.SetParent(liDAROrigin.transform);

            Directory.CreateDirectory(saveFolder + "\\PCD");

            int verticalBeamCount =  (int)(laserScanner.verticalFOV / laserScanner.verticalResolution) +1;
            int horizontalBeamCount = (int)(laserScanner.horizontalFOV / laserScanner.horizontalResolution) +1;

            for (int k = 0 ; k < recordLog.laserLog.laserEntries.Count ; k++)
            {
                liDAROrigin.transform.position = recordLog.laserLog.laserEntries[k].position;
                Quaternion quat = new Quaternion();
                quat.eulerAngles = recordLog.laserLog.laserEntries[k].rotation;
                liDAROrigin.transform.localRotation = quat;

                RaycastHit hit = new RaycastHit();
                List<PCDrow> PCDList = new List<PCDrow>();

                Vector3 laserDirection;

                for(int i = 0 ; i < verticalBeamCount ; i++)
                {
                    Vector3 vertlaserDirection = Quaternion.AngleAxis(laserScanner.verticalFOV / 2 - i * laserScanner.verticalResolution, -liDAR.transform.right) * liDAR.transform.forward;

                    for(int j = 0 ; j < horizontalBeamCount ; j++)
                    {
                        laserDirection = Quaternion.AngleAxis(-laserScanner.horizontalFOV / 2 + j * laserScanner.horizontalResolution, liDAR.transform.up) * vertlaserDirection;

                        //FOR DEBUG
                        //Debug.DrawRay(liDAR.transform.position, laserDirection, new Color(255, 0, 0), 120);

                        bool isHit = Physics.Raycast(liDAR.transform.position, laserDirection, out hit,laserScanner.maxDistance);

                        PCDrow row = new PCDrow();
                        if (isHit && (hit.distance > laserScanner.minDistance))
                        {
                            row.position = hit.point;
                            row.distance = hit.distance;
                        }
                        else
                        {
                            row.position = new Vector3();
                            row.distance = 0.0f;
                        }
                            PCDList.Add(row);

                    }
                }

                PCDGenerator(saveFolder + "\\PCD\\frame_" + k.ToString() + ".pcd" ,PCDList, horizontalBeamCount,verticalBeamCount);

                //FOR DEBUG
                //cameraMode = CameraMode.None;
                //alert.closeAlertDialog();
                //return;
            }

            stopwatch.Stop();
            Debug.Log("<color=blue>PCD FILE GENERATION TIME</color>" + stopwatch.ElapsedMilliseconds);

            cameraButton.interactable = true;
            if (carController != null)
            {
                carController.wheelColliders[0].brakeTorque = 0;
                carController.wheelColliders[1].brakeTorque = 0;
            }
            controllerObject.SetActive(true);
            cameraMode = CameraMode.None;
            alert.closeAlertDialog();
        }
예제 #2
0
        private void generatePCDOperations()
        {
            Button cameraButton = this.transform.Find("Panel").Find("ButtonCameraMode").GetComponent <Button>();

            if (!hasLaserScanner)
            {
                cameraMode = CameraMode.None;
                alert.closeAlertDialog();
                cameraButton = this.transform.Find("Panel").Find("ButtonCameraMode").GetComponent <Button>();
                cameraButton.interactable = true;

                if (carController != null)
                {
                    carController.wheelColliders[0].brakeTorque = 0;
                    carController.wheelColliders[1].brakeTorque = 0;
                }
                controllerObject.SetActive(true);
                return;
            }

            System.Diagnostics.Stopwatch stopwatch = new System.Diagnostics.Stopwatch();
            stopwatch.Start();


            //This is the Game Object contains transform of the camera Van
            liDAROrigin = new GameObject();

            //This is the Game Object contains transform information of the LiDAR device wrt Camera Van
            GameObject liDAR = new GameObject();

            liDAR.transform.position = laserScanner.position;
            Quaternion liDARquat = new Quaternion();

            liDARquat.eulerAngles    = laserScanner.rotation;
            liDAR.transform.rotation = liDARquat;

            //liDAR and Camera Van is Linked
            liDAR.transform.SetParent(liDAROrigin.transform);

            Directory.CreateDirectory(saveFolder + "\\PCD");

            int verticalBeamCount   = (int)(laserScanner.verticalFOV / laserScanner.verticalResolution) + 1;
            int horizontalBeamCount = (int)(laserScanner.horizontalFOV / laserScanner.horizontalResolution) + 1;

            for (int k = 0; k < recordLog.laserLog.laserEntries.Count; k++)
            {
                liDAROrigin.transform.position = recordLog.laserLog.laserEntries[k].position;
                Quaternion quat = new Quaternion();
                quat.eulerAngles = recordLog.laserLog.laserEntries[k].rotation;
                liDAROrigin.transform.localRotation = quat;

                RaycastHit    hit     = new RaycastHit();
                List <PCDrow> PCDList = new List <PCDrow>();

                Vector3 laserDirection;

                for (int i = 0; i < verticalBeamCount; i++)
                {
                    Vector3 vertlaserDirection = Quaternion.AngleAxis(laserScanner.verticalFOV / 2 - i * laserScanner.verticalResolution, -liDAR.transform.right) * liDAR.transform.forward;

                    for (int j = 0; j < horizontalBeamCount; j++)
                    {
                        laserDirection = Quaternion.AngleAxis(-laserScanner.horizontalFOV / 2 + j * laserScanner.horizontalResolution, liDAR.transform.up) * vertlaserDirection;

                        //FOR DEBUG
                        //Debug.DrawRay(liDAR.transform.position, laserDirection, new Color(255, 0, 0), 120);

                        bool isHit = Physics.Raycast(liDAR.transform.position, laserDirection, out hit, laserScanner.maxDistance);

                        PCDrow row = new PCDrow();
                        if (isHit && (hit.distance > laserScanner.minDistance))
                        {
                            row.position = hit.point;
                            row.distance = hit.distance;
                        }
                        else
                        {
                            row.position = new Vector3();
                            row.distance = 0.0f;
                        }
                        PCDList.Add(row);
                    }
                }

                PCDGenerator(saveFolder + "\\PCD\\frame_" + k.ToString() + ".pcd", PCDList, horizontalBeamCount, verticalBeamCount);

                //FOR DEBUG
                //cameraMode = CameraMode.None;
                //alert.closeAlertDialog();
                //return;
            }

            stopwatch.Stop();
            Debug.Log("<color=blue>PCD FILE GENERATION TIME</color>" + stopwatch.ElapsedMilliseconds);

            cameraButton.interactable = true;
            if (carController != null)
            {
                carController.wheelColliders[0].brakeTorque = 0;
                carController.wheelColliders[1].brakeTorque = 0;
            }
            controllerObject.SetActive(true);
            cameraMode = CameraMode.None;
            alert.closeAlertDialog();
        }