123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254 |
- using System.Collections.Generic;
- using UnityEngine;
- using Windows.Kinect;
- public class PointCloudAggregator : MonoBehaviour {
- public float accuracy = 1000f;
- public float maxDistance = 15f;
- public ParticleSystem _particleSystem;
- public CSVExport csvExporter;
- 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;
- public Texture2D cameraTexture;
-
- Dictionary<Vector2Int, ColorHeight> data = new Dictionary<Vector2Int, ColorHeight>();
- Dictionary<Vector3, Color> data2 = new Dictionary<Vector3, Color>();
- List<Vector3> data3 = new List<Vector3>();
- public 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];
-
- }
- }
-
- // Update is called once per frame
- void Update () {
-
- if (Input.GetKeyDown(KeyCode.Space))
- {
- Debug.Log("Snapshot");
- NewSnapshot();
- Debug.Log("Data set now has: " + data.Count + " entries");
- }
- if (Input.GetKeyDown(KeyCode.C))
- {
- Debug.Log("Cloud!");
- Debug.Log(data.Count);
- ShowAsPointCloud();
- }
- if (Input.GetKeyDown(KeyCode.A))
- {
- Debug.Log("Cloud2!");
- Debug.Log(data2.Count);
- ShowAsPointCloud2();
- }
- if (Input.GetKeyDown(KeyCode.W))
- {
- Debug.Log("ToDisk!");
- Debug.Log(data3.Count);
- WriteToDisk();
- }
- if (Input.GetKeyDown(KeyCode.R))
- {
- Debug.Log("Cleared!");
- data = new Dictionary<Vector2Int, ColorHeight>();
- data2 = new Dictionary<Vector3, Color>();
- }
- }
- void NewSnapshot()
- {
- Vector3 currentPoint;
- Vector2Int floorPosition;
- ushort[] rawdata = _MultiManager.GetDepthData();
- Color? tempCol;
- Color validCol;
- 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.TransformPoint(currentPoint);
-
- float x = (Mathf.Clamp(currentPoint.x, -maxDistance, maxDistance ));
- float y = (Mathf.Clamp(currentPoint.z, -maxDistance, maxDistance ));
-
-
- x *= accuracy;
- y *= accuracy;
- if(float.IsNaN(x) || float.IsNaN(y))
- {
- continue;
- }
-
- floorPosition = new Vector2Int(Mathf.RoundToInt(x),Mathf.RoundToInt(y));
- tempCol = GetColorAtIndex(i);
- if (tempCol == null)
- {
- continue;
- }
- else {
- validCol = (Color)tempCol;
- }
- data2[currentPoint] = validCol;
- data3.Add(currentPoint);
- if (data.ContainsKey(floorPosition))
- {
- ColorHeight values = data[floorPosition];
- values.UpdateInfo(currentPoint.y, validCol);
- data[floorPosition] = values;
- }
- else
- {
- data[floorPosition] = new ColorHeight(currentPoint.y, validCol);
- }
-
- }
-
- }
- Vector3 KinectPointToVector3(CameraSpacePoint point)
- {
- return new Vector3(point.X, point.Y, point.Z);
- }
- Color? GetColorAtIndex(int index)
- {
- int x = Mathf.RoundToInt(colorSpacePoints[index].X);
- int y = Mathf.RoundToInt(colorSpacePoints[index].Y);
-
-
- if(y < 0 || y > cameraTexture.height)
- {
- return null;
- }
-
- Color result = cameraTexture.GetPixel(x,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);
- }
- void ShowAsPointCloud2()
- {
- particles = new ParticleSystem.Particle[data2.Count];
- int i = 0;
- foreach (Vector3 pos in data2.Keys)
- {
- particles[i].position = pos;
- particles[i].startColor = data2[pos];
- particles[i].startSize = 0.02f;
- i++;
- }
- _particleSystem.SetParticles(particles, particles.Length);
- }
- void WriteToDisk()
- {
- csvExporter.ExportCSV(data3.ToArray(), "foo");
- }
- }
|