PointCloudAggregator.cs 5.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254
  1. using System.Collections.Generic;
  2. using UnityEngine;
  3. using Windows.Kinect;
  4. public class PointCloudAggregator : MonoBehaviour {
  5. public float accuracy = 1000f;
  6. public float maxDistance = 15f;
  7. public ParticleSystem _particleSystem;
  8. public CSVExport csvExporter;
  9. private ParticleSystem.Particle[] particles;
  10. // Kinectsensor foo
  11. private MultiSourceManager _MultiManager;
  12. private KinectSensor _Sensor;
  13. private CoordinateMapper _Mapper;
  14. private int depthWidth;
  15. private int depthHeight;
  16. private FrameDescription depthFrameDesc;
  17. private CameraSpacePoint[] cameraSpacePoints;
  18. private ColorSpacePoint[] colorSpacePoints;
  19. public Texture2D cameraTexture;
  20. Dictionary<Vector2Int, ColorHeight> data = new Dictionary<Vector2Int, ColorHeight>();
  21. Dictionary<Vector3, Color> data2 = new Dictionary<Vector3, Color>();
  22. List<Vector3> data3 = new List<Vector3>();
  23. public struct ColorHeight
  24. {
  25. public float height;
  26. public Color color;
  27. float avgAbout;
  28. public ColorHeight(float inHeight, Color inColor)
  29. {
  30. height = inHeight;
  31. color = inColor;
  32. avgAbout = 1;
  33. }
  34. public void UpdateInfo(float newHeight, Color newColor)
  35. {
  36. color = (color * avgAbout / (avgAbout + 1)) + newColor / avgAbout;
  37. height = (height * avgAbout / (avgAbout + 1)) + newHeight / avgAbout;
  38. avgAbout++;
  39. }
  40. }
  41. void Start()
  42. {
  43. _Sensor = KinectSensor.GetDefault();
  44. if (_Sensor != null)
  45. {
  46. _MultiManager = GetComponent<MultiSourceManager>();
  47. _Mapper = _Sensor.CoordinateMapper;
  48. depthFrameDesc = _Sensor.DepthFrameSource.FrameDescription;
  49. depthWidth = depthFrameDesc.Width;
  50. depthHeight = depthFrameDesc.Height;
  51. if (!_Sensor.IsOpen)
  52. {
  53. _Sensor.Open();
  54. }
  55. cameraSpacePoints = new CameraSpacePoint[depthWidth * depthHeight];
  56. colorSpacePoints = new ColorSpacePoint[depthWidth * depthHeight];
  57. }
  58. }
  59. // Update is called once per frame
  60. void Update () {
  61. if (Input.GetKeyDown(KeyCode.Space))
  62. {
  63. Debug.Log("Snapshot");
  64. NewSnapshot();
  65. Debug.Log("Data set now has: " + data.Count + " entries");
  66. }
  67. if (Input.GetKeyDown(KeyCode.C))
  68. {
  69. Debug.Log("Cloud!");
  70. Debug.Log(data.Count);
  71. ShowAsPointCloud();
  72. }
  73. if (Input.GetKeyDown(KeyCode.A))
  74. {
  75. Debug.Log("Cloud2!");
  76. Debug.Log(data2.Count);
  77. ShowAsPointCloud2();
  78. }
  79. if (Input.GetKeyDown(KeyCode.W))
  80. {
  81. Debug.Log("ToDisk!");
  82. Debug.Log(data3.Count);
  83. WriteToDisk();
  84. }
  85. if (Input.GetKeyDown(KeyCode.R))
  86. {
  87. Debug.Log("Cleared!");
  88. data = new Dictionary<Vector2Int, ColorHeight>();
  89. data2 = new Dictionary<Vector3, Color>();
  90. }
  91. }
  92. void NewSnapshot()
  93. {
  94. Vector3 currentPoint;
  95. Vector2Int floorPosition;
  96. ushort[] rawdata = _MultiManager.GetDepthData();
  97. Color? tempCol;
  98. Color validCol;
  99. cameraTexture = _MultiManager.GetColorTexture();
  100. _Mapper.MapDepthFrameToCameraSpace(rawdata, cameraSpacePoints);
  101. _Mapper.MapDepthFrameToColorSpace(rawdata, colorSpacePoints);
  102. for(int i = 0; i < cameraSpacePoints.Length; i++)
  103. {
  104. currentPoint = KinectPointToVector3(cameraSpacePoints[i]);
  105. currentPoint = transform.TransformPoint(currentPoint);
  106. float x = (Mathf.Clamp(currentPoint.x, -maxDistance, maxDistance ));
  107. float y = (Mathf.Clamp(currentPoint.z, -maxDistance, maxDistance ));
  108. x *= accuracy;
  109. y *= accuracy;
  110. if(float.IsNaN(x) || float.IsNaN(y))
  111. {
  112. continue;
  113. }
  114. floorPosition = new Vector2Int(Mathf.RoundToInt(x),Mathf.RoundToInt(y));
  115. tempCol = GetColorAtIndex(i);
  116. if (tempCol == null)
  117. {
  118. continue;
  119. }
  120. else {
  121. validCol = (Color)tempCol;
  122. }
  123. data2[currentPoint] = validCol;
  124. data3.Add(currentPoint);
  125. if (data.ContainsKey(floorPosition))
  126. {
  127. ColorHeight values = data[floorPosition];
  128. values.UpdateInfo(currentPoint.y, validCol);
  129. data[floorPosition] = values;
  130. }
  131. else
  132. {
  133. data[floorPosition] = new ColorHeight(currentPoint.y, validCol);
  134. }
  135. }
  136. }
  137. Vector3 KinectPointToVector3(CameraSpacePoint point)
  138. {
  139. return new Vector3(point.X, point.Y, point.Z);
  140. }
  141. Color? GetColorAtIndex(int index)
  142. {
  143. int x = Mathf.RoundToInt(colorSpacePoints[index].X);
  144. int y = Mathf.RoundToInt(colorSpacePoints[index].Y);
  145. if(y < 0 || y > cameraTexture.height)
  146. {
  147. return null;
  148. }
  149. Color result = cameraTexture.GetPixel(x,y);
  150. return result;
  151. }
  152. void ShowAsPointCloud()
  153. {
  154. ColorHeight colorHeight;
  155. particles = new ParticleSystem.Particle[data.Count];
  156. int i = 0;
  157. foreach(Vector2Int floorPos in data.Keys)
  158. {
  159. colorHeight = data[floorPos];
  160. particles[i].position = new Vector3((float)floorPos.x/accuracy, (float)colorHeight.height, (float)floorPos.y/ accuracy);
  161. particles[i].startColor = colorHeight.color;
  162. particles[i].startSize = 0.02f;
  163. i++;
  164. }
  165. _particleSystem.SetParticles(particles, particles.Length);
  166. }
  167. void ShowAsPointCloud2()
  168. {
  169. particles = new ParticleSystem.Particle[data2.Count];
  170. int i = 0;
  171. foreach (Vector3 pos in data2.Keys)
  172. {
  173. particles[i].position = pos;
  174. particles[i].startColor = data2[pos];
  175. particles[i].startSize = 0.02f;
  176. i++;
  177. }
  178. _particleSystem.SetParticles(particles, particles.Length);
  179. }
  180. void WriteToDisk()
  181. {
  182. csvExporter.ExportCSV(data3.ToArray(), "foo");
  183. }
  184. }