Raummodell-in-VR/VR Raumrekonstuktion/Assets/Mine/Scripts/PointCloudAggregator.cs

using System.Collections.Generic;
using UnityEngine;
using Windows.Kinect;


public class PointCloudAggregator : MonoBehaviour {
	int minY = 0;
	int maxY = 0;
	public float accuracy = 1000f;
	public float maxDistance = 15f;

	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;
	public Texture2D cameraTexture;
	private Color[] cameraColor;

	Dictionary<Vector2Int, ColorHeight> data = new Dictionary<Vector2Int, ColorHeight>();

	Dictionary<Vector3, Color> data2 = new Dictionary<Vector3, Color>();

	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];

			cameraColor = new Color[depthWidth * depthHeight];
		}
	}
	
	// Update is called once per frame
	void Update () {
		
		if (Input.GetKeyDown(KeyCode.Space))
		{
			Debug.Log("Snapshot");
			NewSnapshot();
			Debug.Log("Data set not 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(data.Count);
			ShowAsPointCloud2();
		}

		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;


			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);
	}

}