ZED3DObjectVisualizer.cs 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480
  1. using System;
  2. using System.Collections;
  3. using System.Collections.Generic;
  4. using System.Linq;
  5. using Assets.Logging;
  6. using Assets.ZED.SDK.Helpers.Scripts;
  7. using UnityEngine;
  8. /// <summary>
  9. /// For the ZED 3D Object Detection sample.
  10. /// Listens for new object detections (via the ZEDManager.OnObjectDetection event) and moves + resizes cube prefabs
  11. /// to represent them.
  12. /// <para>Works by instantiating a pool of prefabs, and each frame going through the DetectedFrame received from the event
  13. /// to make sure each detected object has a representative GameObject. Also disables GameObjects whose objects are no
  14. /// longer visible and returns them to the pool.</para>
  15. /// </summary>
  16. public class ZED3DObjectVisualizer : MonoBehaviour
  17. {
  18. /// <summary>
  19. /// The scene's ZEDManager.
  20. /// If you want to visualize detections from multiple ZEDs at once you will need multiple ZED3DObjectVisualizer commponents in the scene.
  21. /// </summary>
  22. [Tooltip("The scene's ZEDManager.\r\n" +
  23. "If you want to visualize detections from multiple ZEDs at once you will need multiple ZED3DObjectVisualizer commponents in the scene. ")]
  24. IZEDManager ZedManager => zedManagerLazy.Value;
  25. Lazy<IZEDManager> zedManagerLazy;
  26. /// <summary>
  27. /// If true, the ZED Object Detection manual will be started as soon as the ZED is initiated.
  28. /// This avoids having to press the Start Object Detection button in ZEDManager's Inspector.
  29. /// </summary>
  30. [Tooltip("If true, the ZED Object Detection manual will be started as soon as the ZED is initiated. " +
  31. "This avoids having to press the Start Object Detection button in ZEDManager's Inspector.")]
  32. public bool startObjectDetectionAutomatically = true;
  33. /// <summary>
  34. /// Prefab object that's instantiated to represent detected objects.
  35. /// This class expects the object to have the default Unity cube as a mesh - otherwise, it may be scaled incorrectly.
  36. /// It also expects a BBox3DHandler component in the root object, but you won't have errors if it lacks one.
  37. /// </summary>
  38. [Space(5)]
  39. [Header("Box Appearance")]
  40. [Tooltip("Prefab object that's instantiated to represent detected objects. " +
  41. "This class expects the object to have the default Unity cube as a mesh - otherwise, it may be scaled incorrectly.\r\n" +
  42. "It also expects a BBox3DHandler component in the root object, but you won't have errors if it lacks one. ")]
  43. public GameObject boundingBoxPrefab;
  44. /// <summary>
  45. /// The colors that will be cycled through when assigning colors to new bounding boxes.
  46. /// </summary>
  47. [Tooltip("The colors that will be cycled through when assigning colors to new bounding boxes. ")]
  48. //[ColorUsage(true, true)] //Uncomment to enable HDR colors in versions of Unity that support it.
  49. public List<Color> boxColors = new List<Color>()
  50. {
  51. new Color(.231f, .909f, .69f, 1),
  52. new Color(.098f, .686f, .816f, 1),
  53. new Color(.412f, .4f, .804f, 1),
  54. new Color(1, .725f, 0f, 1),
  55. new Color(.989f, .388f, .419f, 1)
  56. };
  57. /// <summary>
  58. /// If true, bounding boxes are rotated to face the camera that detected them. This has more parity with the SDK and will generally result in more accurate boxes.
  59. /// If false, the boxes are calculated from known bounds to face Z = 1.
  60. /// </summary>
  61. [Space(5)]
  62. [Header("Box Transform")]
  63. [Tooltip("If true, bounding boxes are rotated to face the camera that detected them. If false, the boxes are calculated from known bounds to face Z = 1. " +
  64. "'False' has more parity with the SDK and will generally result in more accurate boxes.")]
  65. public bool boxesFaceCamera = false;
  66. /// <summary>
  67. /// If true, transforms the localScale of the root bounding box transform to match the detected 3D bounding box.
  68. /// </summary>
  69. [Tooltip("If true, transforms the localScale of the root bounding box transform to match the detected 3D bounding box. ")]
  70. public bool transformBoxScale = true;
  71. /// <summary>
  72. /// If true, and transformBoxScale is also true, modifies the center and Y bounds of each detected bounding box so that its
  73. /// bottom is at floor level (Y = 0) while keeping the other corners at the same place.
  74. /// </summary>
  75. [Tooltip("If true, and transformBoxScale is also true, modifies the center and Y bounds of each detected bounding box so that its " +
  76. "bottom is at floor level (Y = 0) while keeping the other corners at the same place. WARNING: Estimate Initial position must be set to true.")]
  77. public bool transformBoxToTouchFloor = true;
  78. /// <summary>
  79. /// If true, sets the Y value of the center of the bounding box to 0. Use for bounding box prefabs meant to be centered at the user's feet.
  80. /// </summary>
  81. [Tooltip("If true, sets the Y value of the center of the bounding box to 0. Use for bounding box prefabs meant to be centered at the user's feet. ")]
  82. [LabelOverride("Box Center Always On Floor")]
  83. public bool floorBBoxPosition = false;
  84. /// <summary>
  85. /// Display bounding boxes of objects that are actively being tracked by object tracking, where valid positions are known.
  86. /// </summary>
  87. [Space(5)]
  88. [Header("Filters")]
  89. [Tooltip("Display bounding boxes of objects that are actively being tracked by object tracking, where valid positions are known. ")]
  90. public bool showONTracked = true;
  91. /// <summary>
  92. /// Display bounding boxes of objects that were actively being tracked by object tracking, but that were lost very recently.
  93. /// </summary>
  94. [Tooltip("Display bounding boxes of objects that were actively being tracked by object tracking, but that were lost very recently.")]
  95. public bool showSEARCHINGTracked = false;
  96. /// <summary>
  97. /// Display bounding boxes of objects that are visible but not actively being tracked by object tracking (usually because object tracking is disabled in ZEDManager).
  98. /// </summary>
  99. [Tooltip("Display bounding boxes of objects that are visible but not actively being tracked by object tracking (usually because object tracking is disabled in ZEDManager).")]
  100. public bool showOFFTracked = false;
  101. /// <summary>
  102. /// How wide a bounding box has to be in order to be displayed. Use this to remove tiny bounding boxes from partially-occluded objects.
  103. /// (If you have this issue, it can also be helpful to set showSEARCHINGTracked to OFF.)
  104. /// </summary>
  105. [Tooltip("How wide a bounding box has to be in order to be displayed. Use this to remove tiny bounding boxes from partially-occluded objects.\r\n" +
  106. "(If you have this issue, it can also be helpful to set showSEARCHINGTracked to OFF.)")]
  107. public float minimumWidthToDisplay = 0.3f;
  108. /// <summary>
  109. /// When a detected object is first given a box and assigned a color, we store it so that if the object
  110. /// disappears and appears again later, it's assigned the same color.
  111. /// This is also solvable by making the color a function of the ID number itself, but then you can get
  112. /// repeat colors under certain conditions.
  113. /// </summary>
  114. private Dictionary<int, Color> idColorDict = new Dictionary<int, Color>();
  115. /// <summary>
  116. /// Pre-instantiated bbox prefabs currently not in use.
  117. /// </summary>
  118. private Stack<GameObject> bboxPool = new Stack<GameObject>();
  119. /// <summary>
  120. /// All active GameObjects that were instantiated to the prefab and that currently represent a detected object.
  121. /// Key is the object's objectID.
  122. /// </summary>
  123. private Dictionary<int, GameObject> liveBBoxes = new Dictionary<int, GameObject>();
  124. /// <summary>
  125. /// Used to know which of the available colors will be assigned to the next bounding box to be used.
  126. /// </summary>
  127. private int nextColorIndex = 0;
  128. private void Awake()
  129. {
  130. zedManagerLazy = new(new LoggingZEDManager(FindObjectOfType<ZEDManager>(), new DetectionFrameLogger()));
  131. }
  132. // Use this for initialization
  133. void Start()
  134. {
  135. ZedManager.OnObjectDetection += ZedManager_OnObjectDetection;
  136. ZedManager.OnZEDReady += OnZEDReady;
  137. if (ZedManager.EstimateInitialPosition == false && transformBoxToTouchFloor == true)
  138. {
  139. Debug.Log("Estimate initial position is set to false. Then, transformBoxToTouchFloor is disable.");
  140. transformBoxToTouchFloor = false;
  141. }
  142. }
  143. private void ZedManager_OnObjectDetection(DetectionFrame objFrame)
  144. {
  145. Visualize3DBoundingBoxes(objFrame);
  146. UpdateMinMaxCoordinates(objFrame);
  147. }
  148. private void UpdateMinMaxCoordinates(DetectionFrame objFrame)
  149. {
  150. List<DetectedObject> newobjects = objFrame.GetFilteredObjectList(showONTracked, showSEARCHINGTracked, showOFFTracked);
  151. foreach (var detectedObject in newobjects)
  152. {
  153. Bounds objbounds = detectedObject.Get3DWorldBounds();
  154. if (objbounds.size.x < minimumWidthToDisplay || objbounds.size == Vector3.zero) { }
  155. {
  156. Vector3 obj_position = detectedObject.Get3DWorldPosition();
  157. minX = Math.Min(minX, obj_position.x);
  158. maxX = Math.Max(maxX, obj_position.x);
  159. minY = Math.Min(minY, obj_position.y);
  160. maxY = Math.Max(maxY, obj_position.y);
  161. minZ = Math.Min(minZ, obj_position.z);
  162. maxZ = Math.Max(maxZ, obj_position.z);
  163. }
  164. }
  165. }
  166. private void OnZEDReady()
  167. {
  168. if (startObjectDetectionAutomatically && !ZedManager.IsObjectDetectionRunning)
  169. {
  170. ZedManager.StartObjectDetection();
  171. }
  172. }
  173. float maxX = 0.9971107f;
  174. float minX = -0.7968294f;
  175. float maxY = int.MinValue;
  176. float minY = int.MaxValue;
  177. float maxZ = 2.373606f;
  178. float minZ = 0.4380694f;
  179. /// <summary>
  180. /// Given a frame of object detections, positions a GameObject to represent every visible object
  181. /// in that object's actual 3D location within the world.
  182. /// <para>Called from ZEDManager.OnObjectDetection each time there's a new detection frame available.</para>
  183. /// </summary>
  184. private void Visualize3DBoundingBoxes(DetectionFrame dframe)
  185. {
  186. //Get a list of all active IDs from last frame, and we'll remove each box that's visible this frame.
  187. //At the end, we'll clear the remaining boxes, as those are objects no longer visible to the ZED.
  188. List<int> activeids = liveBBoxes.Keys.ToList();
  189. List<DetectedObject> newobjects = dframe.GetFilteredObjectList(showONTracked, showSEARCHINGTracked, showOFFTracked);
  190. var ball = GameObject.Find("Sphere").GetComponent<Ball>();
  191. if (ball.transform.position.x > -15 && ball.transform.position.x < 15 && newobjects.Any())
  192. {
  193. if (newobjects.Count >= 2 && !ball.IsInUse)
  194. {
  195. ball.Kickoff();
  196. }
  197. int count = 1;
  198. foreach (var dobj in newobjects.OrderByDescending(i => i.Get3DWorldPosition().x).Take(2))
  199. {
  200. GameObject bump = GameObject.Find($"Bump{count}");
  201. Bounds objbounds = dobj.Get3DWorldBounds();
  202. //Make sure the object is big enough to count. We filter out very small boxes.
  203. if (objbounds.size.x < minimumWidthToDisplay || objbounds.size == Vector3.zero) { }
  204. {
  205. //Remove the ID from the list we'll use to clear no-longer-visible boxes.
  206. if (activeids.Contains(dobj.id)) activeids.Remove(dobj.id);
  207. //Get the box and update its distance value.
  208. GameObject bbox = GetBBoxForObject(dobj);
  209. //Move the box into position.
  210. Vector3 obj_position = dobj.Get3DWorldPosition();
  211. Debug.Log($"count: {count}; X: {obj_position.x}; Y: {obj_position.y}; Z: {obj_position.z};");
  212. Debug.Log($"x: ({minX}|{maxX}), y: ({minY}|{maxY}), z: ({minZ}|{maxZ}), ");
  213. if (!ZEDSupportFunctions.IsVector3NaN(obj_position))
  214. {
  215. bbox.transform.position = obj_position;
  216. if (floorBBoxPosition)
  217. {
  218. bbox.transform.position = new Vector3(bbox.transform.position.x, 0, bbox.transform.position.z);
  219. }
  220. var x = bbox.transform.position.x;
  221. var totalDistance = maxX - minX;
  222. var distanceFromStart = x - minX;
  223. var ratio = distanceFromStart / totalDistance;
  224. var newX = -12 + ratio * 24;
  225. var z = bbox.transform.position.z;
  226. totalDistance = maxZ - minZ;
  227. distanceFromStart = z - minZ;
  228. ratio = distanceFromStart / totalDistance;
  229. var newZ = -6 + ratio * 12;
  230. bbox.transform.position = new Vector3(newX, 0, newZ);
  231. bbox.transform.rotation = dobj.Get3DWorldRotation(boxesFaceCamera); //Rotate them.
  232. if (bbox.transform.position.z > -5 && bbox.transform.position.z < 5)
  233. {
  234. bump.transform.position = new Vector3(bump.transform.position.x, bump.transform.position.y, bbox.transform.position.z);
  235. }
  236. }
  237. //Transform the box if desired.
  238. if (transformBoxScale)
  239. {
  240. //We'll scale the object assuming that it's mesh is the default Unity cube, or something sized equally.
  241. if (transformBoxToTouchFloor)
  242. {
  243. Vector3 startscale = objbounds.size;
  244. float distfromfloor = bbox.transform.position.y - (objbounds.size.y / 2f);
  245. bbox.transform.localScale = new Vector3(objbounds.size.x, objbounds.size.y + distfromfloor, objbounds.size.z);
  246. Vector3 newpos = bbox.transform.position;
  247. newpos.y -= (distfromfloor / 2f);
  248. bbox.transform.position = newpos;
  249. }
  250. else
  251. {
  252. bbox.transform.localScale = objbounds.size;
  253. }
  254. }
  255. //Now that we've adjusted position, tell the handler on the prefab to adjust distance display..
  256. BBox3DHandler boxhandler = bbox.GetComponent<BBox3DHandler>();
  257. if (boxhandler)
  258. {
  259. float disttobox = Vector3.Distance(dobj.detectingZEDManager.GetLeftCameraTransform().position, dobj.Get3DWorldPosition());
  260. boxhandler.SetDistance(disttobox);
  261. boxhandler.UpdateBoxUVScales();
  262. boxhandler.UpdateLabelScaleAndPosition();
  263. }
  264. //DrawDebugBox(dobj);
  265. }
  266. //Remove boxes for objects that the ZED can no longer see.
  267. foreach (int id in activeids)
  268. {
  269. ReturnBoxToPool(id, liveBBoxes[id]);
  270. }
  271. count++;
  272. }
  273. }
  274. else
  275. {
  276. ball.Recenter();
  277. }
  278. }
  279. /// <summary>
  280. /// Returs the GameObject (instantiated from boundingBoxPrefab) that represents the provided DetectedObject.
  281. /// If none exists, it retrieves one from the pool (or instantiates a new one if none is available) and
  282. /// sets it up with the proper ID and colors.
  283. /// </summary>
  284. private GameObject GetBBoxForObject(DetectedObject dobj)
  285. {
  286. if (!liveBBoxes.ContainsKey(dobj.id))
  287. {
  288. GameObject newbox = GetAvailableBBox();
  289. newbox.name = "Object #" + dobj.id;
  290. BBox3DHandler boxhandler = newbox.GetComponent<BBox3DHandler>();
  291. Color col;
  292. if (idColorDict.ContainsKey(dobj.id))
  293. {
  294. col = idColorDict[dobj.id];
  295. }
  296. else
  297. {
  298. col = GetNextColor();
  299. idColorDict.Add(dobj.id, col);
  300. }
  301. if (boxhandler)
  302. {
  303. boxhandler.SetColor(col);
  304. if (ZedManager.ObjectDetectionModel == sl.DETECTION_MODEL.CUSTOM_BOX_OBJECTS)
  305. {
  306. //boxhandler.SetID(dobj.rawObjectData.rawLabel.ToString());
  307. boxhandler.SetID(dobj.id.ToString());
  308. }
  309. else
  310. {
  311. boxhandler.SetID(dobj.id.ToString());
  312. }
  313. }
  314. liveBBoxes[dobj.id] = newbox;
  315. return newbox;
  316. }
  317. else return liveBBoxes[dobj.id];
  318. }
  319. /// <summary>
  320. /// Gets an available GameObject (instantiated from boundingBoxPrefab) from the pool,
  321. /// or instantiates a new one if none are available.
  322. /// </summary>
  323. /// <returns></returns>
  324. private GameObject GetAvailableBBox()
  325. {
  326. if (bboxPool.Count == 0)
  327. {
  328. GameObject newbbox = Instantiate(boundingBoxPrefab);
  329. newbbox.transform.SetParent(transform, false);
  330. bboxPool.Push(newbbox);
  331. }
  332. GameObject bbox = bboxPool.Pop();
  333. bbox.SetActive(true);
  334. return bbox;
  335. }
  336. /// <summary>
  337. /// Disables a GameObject that was being used to represent an object (of the given id) and puts it back
  338. /// into the pool for later use.
  339. /// </summary>
  340. private void ReturnBoxToPool(int id, GameObject bbox)
  341. {
  342. bbox.SetActive(false);
  343. bbox.name = "Unused";
  344. bboxPool.Push(bbox);
  345. if (liveBBoxes.ContainsKey(id))
  346. {
  347. liveBBoxes.Remove(id);
  348. }
  349. else
  350. {
  351. Debug.LogError("Tried to remove object ID " + id + " from active bboxes, but it wasn't in the dictionary.");
  352. }
  353. }
  354. /// <summary>
  355. /// Returns a color from the boxColors list.
  356. /// Colors are returned sequentially in order of their appearance in that list.
  357. /// </summary>
  358. /// <returns></returns>
  359. private Color GetNextColor()
  360. {
  361. if (boxColors.Count == 0)
  362. {
  363. return new Color(.043f, .808f, .435f, 1);
  364. }
  365. if (nextColorIndex >= boxColors.Count)
  366. {
  367. nextColorIndex = 0;
  368. }
  369. Color returncol = boxColors[nextColorIndex];
  370. nextColorIndex++;
  371. return returncol;
  372. }
  373. private void OnDestroy()
  374. {
  375. if (ZedManager != null)
  376. {
  377. ZedManager.OnObjectDetection -= ZedManager_OnObjectDetection;
  378. ZedManager.OnZEDReady -= OnZEDReady;
  379. }
  380. }
  381. /// <summary>
  382. /// Draws a bounding box in the Scene window. Useful for debugging a 3D bbox's position relative to it.
  383. /// </summary>
  384. private void DrawDebugBox(DetectedObject dobj)
  385. {
  386. //Test bbox orientation.
  387. Transform camtrans = dobj.detectingZEDManager.GetLeftCameraTransform();
  388. Vector3[] corners = dobj.rawObjectData.worldBoundingBox;
  389. Vector3[] rotcorners = new Vector3[8];
  390. //Vector3[] corners3d = new Vector3[8];
  391. Vector3[] corners3d = dobj.Get3DWorldCorners();
  392. for (int i = 0; i < 8; i++)
  393. {
  394. Vector3 fixrot = camtrans.InverseTransformPoint(corners[i]);
  395. rotcorners[i] = fixrot;
  396. }
  397. Debug.DrawLine(corners3d[0], corners3d[1], Color.red);
  398. Debug.DrawLine(corners3d[1], corners3d[2], Color.red);
  399. Debug.DrawLine(corners3d[2], corners3d[3], Color.red);
  400. Debug.DrawLine(corners3d[3], corners3d[0], Color.red);
  401. Debug.DrawLine(corners3d[4], corners3d[5], Color.red);
  402. Debug.DrawLine(corners3d[5], corners3d[6], Color.red);
  403. Debug.DrawLine(corners3d[6], corners3d[7], Color.red);
  404. Debug.DrawLine(corners3d[7], corners3d[4], Color.red);
  405. Debug.DrawLine(corners3d[0], corners3d[4], Color.red);
  406. Debug.DrawLine(corners3d[1], corners3d[5], Color.red);
  407. Debug.DrawLine(corners3d[2], corners3d[6], Color.red);
  408. Debug.DrawLine(corners3d[3], corners3d[7], Color.red);
  409. }
  410. }