123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160 |
- using System.Collections.Generic;
- using UnityEngine;
- using Windows.Kinect;
- public class PointCloudAggregator : MonoBehaviour {
- public float accuracy = 1000f;
- public ParticleSystem _particleSystem;
- private ParticleSystem.Particle[] particles;
- // Kinectsensor foo
- private MultiSourceManager _MultiManager;
- private KinectSensor _Sensor;
- private CoordinateMapper _Mapper;
- private int depthWidth;
- private int depthHeight;
- private FrameDescription depthFrameDesc;
- private CameraSpacePoint[] cameraSpacePoints;
- private ColorSpacePoint[] colorSpacePoints;
- private Texture2D cameraTexture;
- private Color[] cameraColor;
- Dictionary<Vector2Int, ColorHeight> data = new Dictionary<Vector2Int, ColorHeight>();
- struct ColorHeight
- {
- public float height;
- public Color color;
- float avgAbout;
- public ColorHeight(float inHeight, Color inColor)
- {
- height = inHeight;
- color = inColor;
- avgAbout = 1;
- }
- public void UpdateInfo(float newHeight, Color newColor)
- {
- color = (color * avgAbout / (avgAbout + 1)) + newColor / avgAbout;
- height = (height * avgAbout / (avgAbout + 1)) + newHeight / avgAbout;
- avgAbout++;
- }
- }
- void Start()
- {
- _Sensor = KinectSensor.GetDefault();
- if (_Sensor != null)
- {
- _MultiManager = GetComponent<MultiSourceManager>();
- _Mapper = _Sensor.CoordinateMapper;
-
- depthFrameDesc = _Sensor.DepthFrameSource.FrameDescription;
- depthWidth = depthFrameDesc.Width;
- depthHeight = depthFrameDesc.Height;
- if (!_Sensor.IsOpen)
- {
- _Sensor.Open();
- }
- cameraSpacePoints = new CameraSpacePoint[depthWidth * depthHeight];
- colorSpacePoints = new ColorSpacePoint[depthWidth * depthHeight];
- cameraColor = new Color[depthWidth * depthHeight];
- }
- }
-
- // Update is called once per frame
- void Update () {
- if (Input.GetKeyDown(KeyCode.Space))
- {
- Debug.Log("Snapshot");
- NewSnapshot();
- }
- if (Input.GetKeyDown(KeyCode.A))
- {
- Debug.Log("Cloud!");
- Debug.Log(data.Count);
- ShowAsPointCloud();
- }
- }
- void NewSnapshot()
- {
- Vector3 currentPoint;
- Vector2Int floorPosition;
- ushort[] rawdata = _MultiManager.GetDepthData();
- cameraTexture = _MultiManager.GetColorTexture();
- _Mapper.MapDepthFrameToCameraSpace(rawdata, cameraSpacePoints);
- _Mapper.MapDepthFrameToColorSpace(rawdata, colorSpacePoints);
- for(int i = 0; i < cameraSpacePoints.Length; i++)
- {
-
-
- currentPoint = KinectPointToVector3(cameraSpacePoints[i]);
- currentPoint = transform.InverseTransformPoint(currentPoint);
- floorPosition = new Vector2Int(Mathf.RoundToInt(currentPoint.x * (int)accuracy), Mathf.RoundToInt(currentPoint.z * (int)accuracy));
- if (data.ContainsKey(floorPosition))
- {
- ColorHeight values = data[floorPosition];
- values.UpdateInfo(currentPoint.y, GetColorAtIndex(i));
- data[floorPosition] = values;
- }
- else
- {
- data[floorPosition] = new ColorHeight(currentPoint.y, GetColorAtIndex(i));
- }
- }
- }
- Vector3 KinectPointToVector3(CameraSpacePoint point)
- {
- return new Vector3(-point.X, point.Y, point.Z);
- }
- Color GetColorAtIndex(int index)
- {
- Color result = cameraTexture.GetPixel(Mathf.RoundToInt(colorSpacePoints[index].X), Mathf.RoundToInt(colorSpacePoints[index].Y));
- return result;
- }
- void ShowAsPointCloud()
- {
- ColorHeight colorHeight;
- particles = new ParticleSystem.Particle[data.Count];
-
- int i = 0;
- foreach(Vector2Int floorPos in data.Keys)
- {
- colorHeight = data[floorPos];
- particles[i].position = new Vector3((float)floorPos.x/accuracy, (float)colorHeight.height, (float)floorPos.y/ accuracy);
- particles[i].startColor = colorHeight.color;
- particles[i].startSize = 0.02f;
- i++;
- }
- _particleSystem.SetParticles(particles, particles.Length);
- }
- }
|