ZEDFusedPointCloudRenderer.cs 7.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221
  1. //======= Copyright (c) Stereolabs Corporation, All rights reserved. ===============
  2. using UnityEngine;
  3. /// <summary>
  4. /// Displays the point cloud of the real world in front of the camera.
  5. /// Can be attached to any GameObject in a scene, but requires a ZEDManager component to exist somewhere.
  6. /// </summary>
  7. ///
  8. namespace sl
  9. {
  10. public class ZEDFusedPointCloudRenderer : MonoBehaviour
  11. {
  12. /// <summary>
  13. /// Instance of the ZEDManager interface
  14. /// </summary>
  15. public ZEDManager zedManager = null;
  16. /// <summary>
  17. /// Point size of the Fused Point cloud. If > 0, then disk shader will be used to generate spheres around points
  18. /// </summary>
  19. public float _pointSize = 0.001f;
  20. /// <summary>
  21. /// Fused point cloud resolution (sent to spatial mapping parameters)
  22. /// </summary>
  23. public float resolution = 0.5f;
  24. /// <summary>
  25. /// Fused point cloud range (sent to spatial mapping parameters)
  26. /// </summary>
  27. public float range = 5.0f;
  28. /// <summary>
  29. /// zed Camera controller by zedManager.
  30. /// </summary>
  31. private sl.ZEDCamera zed = null;
  32. /// <summary>
  33. /// Point Material
  34. /// </summary>
  35. private Material _pointMaterial;
  36. /// <summary>
  37. /// Disk material (sphere around point)
  38. /// </summary>
  39. private Material _diskMaterial;
  40. /// <summary>
  41. /// Array of points as point buffer (compute buffer). Send to shaders
  42. /// </summary>
  43. private ComputeBuffer _pointBuffer;
  44. /// <summary>
  45. /// Size of Vector4
  46. /// </summary>
  47. private const int elementSize = sizeof(float) * 4;
  48. /// <summary>
  49. /// Array of vertex points
  50. /// </summary>
  51. private Vector4[] vertPoints;
  52. private bool notStarted = true;
  53. private bool canUpdate = false;
  54. private float updateTime = 0;
  55. void Start()
  56. {
  57. if (zedManager == null)
  58. {
  59. zedManager = FindObjectOfType<ZEDManager>();
  60. if (ZEDManager.GetInstances().Count > 1) //We chose a ZED arbitrarily, but there are multiple cams present. Warn the user.
  61. {
  62. Debug.Log("Warning: " + gameObject.name + "'s zedManager was not specified, so the first available ZEDManager instance was " +
  63. "assigned. However, there are multiple ZEDManager's in the scene. It's recommended to specify which ZEDManager you want to " +
  64. "use to display a point cloud.");
  65. }
  66. }
  67. if (zedManager != null)
  68. zed = zedManager.zedCamera;
  69. if (_pointMaterial == null)
  70. {
  71. _pointMaterial = new Material(Resources.Load("Materials/PointCloud/Mat_ZED_FusedPC_Point") as Material);
  72. }
  73. if (_diskMaterial == null)
  74. {
  75. _diskMaterial = new Material(Resources.Load("Materials/PointCloud/Mat_ZED_FusedPC_Disk") as Material);
  76. }
  77. _diskMaterial.hideFlags = HideFlags.DontSave;
  78. _pointMaterial.hideFlags = HideFlags.DontSave;
  79. zedManager.OnGrab += startMap;
  80. }
  81. /// <summary>
  82. /// Start Map linked to grab callback. Wait for one frame to start point cloud fusion
  83. /// </summary>
  84. private void startMap()
  85. {
  86. if (zed != null && notStarted)
  87. {
  88. zed.EnableSpatialMapping(sl.SPATIAL_MAP_TYPE.FUSED_POINT_CLOUD, resolution, range);
  89. notStarted = false;
  90. canUpdate = true;
  91. }
  92. }
  93. void OnDestroy()
  94. {
  95. canUpdate = false;
  96. if (zed != null)
  97. zed.DisableSpatialMapping();
  98. if (_pointBuffer != null)
  99. {
  100. _pointBuffer.Release();
  101. _pointBuffer = null;
  102. }
  103. if (_pointMaterial != null)
  104. {
  105. if (Application.isPlaying)
  106. {
  107. Destroy(_pointMaterial);
  108. Destroy(_diskMaterial);
  109. }
  110. else
  111. {
  112. DestroyImmediate(_pointMaterial);
  113. DestroyImmediate(_diskMaterial);
  114. }
  115. }
  116. }
  117. /// <summary>
  118. /// Update to request Map every 1 second
  119. /// </summary>
  120. void Update()
  121. {
  122. if (zed.IsCameraReady && canUpdate) //Don't do anything unless the ZED has been initialized.
  123. {
  124. updateTime += Time.deltaTime;
  125. if (updateTime >= 1)
  126. {
  127. zed.RequestMesh();
  128. updateTime = 0;
  129. }
  130. }
  131. }
  132. /// <summary>
  133. /// On Render Fct
  134. /// </summary>
  135. void OnRenderObject()
  136. {
  137. if (zed != null)
  138. {
  139. int nbPoints = 0;
  140. zed.UpdateFusedPointCloud(ref nbPoints);
  141. if (nbPoints > 0)
  142. {
  143. vertPoints = new Vector4[nbPoints];
  144. zed.RetrieveFusedPointCloud(vertPoints);
  145. if (_pointBuffer != null)
  146. {
  147. _pointBuffer.Release();
  148. _pointBuffer = null;
  149. }
  150. _pointBuffer = new ComputeBuffer(nbPoints, elementSize);
  151. _pointBuffer.SetData(vertPoints);
  152. }
  153. }
  154. if (_pointBuffer != null)
  155. {
  156. //Draw with Point shader
  157. if (_pointSize == 0)
  158. {
  159. _pointMaterial.SetPass(0);
  160. _pointMaterial.SetMatrix("_Transform", transform.localToWorldMatrix);
  161. _pointMaterial.SetBuffer("_PointBuffer", _pointBuffer);
  162. #if UNITY_2019_1_OR_NEWER
  163. Graphics.DrawProceduralNow(MeshTopology.Points, _pointBuffer.count, 1);
  164. #else
  165. Graphics.DrawProcedural(MeshTopology.Points, _pointBuffer.count, 1);
  166. #endif
  167. }
  168. //Draw with Disk shader
  169. else
  170. {
  171. _diskMaterial.SetPass(0);
  172. _diskMaterial.SetMatrix("_Transform", transform.localToWorldMatrix);
  173. _diskMaterial.SetBuffer("_PointBuffer", _pointBuffer);
  174. _diskMaterial.SetFloat("_PointSize", _pointSize);
  175. #if UNITY_2019_1_OR_NEWER
  176. Graphics.DrawProceduralNow(MeshTopology.Points, _pointBuffer.count, 1);
  177. #else
  178. Graphics.DrawProcedural(MeshTopology.Points, _pointBuffer.count, 1);
  179. #endif
  180. }
  181. }
  182. }
  183. }
  184. }