StreetLight/Assets/StreetLight/MyZED3DObjectVisualizer.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using Assets.Logging;
using Assets.StreetLight.Poco;
using Assets.StreetLight.Scripts;
using Assets.ZED.SDK.Helpers.Scripts;
using UnityEngine;

/// <summary>
/// For the ZED 3D Object Detection sample.
/// Listens for new object detections (via the ZEDManager.OnObjectDetection event) and moves + resizes cube prefabs
/// to represent them.
/// <para>Works by instantiating a pool of prefabs, and each frame going through the DetectedFrame received from the event
/// to make sure each detected object has a representative GameObject. Also disables GameObjects whose objects are no
/// longer visible and returns them to the pool.</para>
/// </summary>
public class MyZED3DObjectVisualizer : MonoBehaviour
{
    /// <summary>
    /// Prefab object that's instantiated to represent detected objects.
    /// This class expects the object to have the default Unity cube as a mesh - otherwise, it may be scaled incorrectly.
    /// It also expects a BBox3DHandler component in the root object, but you won't have errors if it lacks one.
    /// </summary>
    [Space(5)]
    [Header("Box Appearance")]
    [Tooltip("Prefab object that's instantiated to represent detected objects. " +
        "This class expects the object to have the default Unity cube as a mesh - otherwise, it may be scaled incorrectly.\r\n" +
        "It also expects a BBox3DHandler component in the root object, but you won't have errors if it lacks one. ")]
    public GameObject boundingBoxPrefab;


    /// <summary>
    /// The colors that will be cycled through when assigning colors to new bounding boxes.
    /// </summary>
    [Tooltip("The colors that will be cycled through when assigning colors to new bounding boxes. ")]
    //[ColorUsage(true, true)] //Uncomment to enable HDR colors in versions of Unity that support it.
    public List<Color> boxColors = new List<Color>()
    {
        new Color(.231f, .909f, .69f, 1),
        new Color(.098f, .686f, .816f, 1),
        new Color(.412f, .4f, .804f, 1),
        new Color(1, .725f, 0f, 1),
        new Color(.989f, .388f, .419f, 1)
    };


    /// <summary>
    /// 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.
    /// If false, the boxes are calculated from known bounds to face Z = 1.
    /// </summary>
    [Space(5)]
    [Header("Box Transform")]
    [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. " +
        "'False' has more parity with the SDK and will generally result in more accurate boxes.")]
    public bool boxesFaceCamera = false;
    /// <summary>
    /// If true, transforms the localScale of the root bounding box transform to match the detected 3D bounding box.
    /// </summary>
    [Tooltip("If true, transforms the localScale of the root bounding box transform to match the detected 3D bounding box. ")]
    public bool transformBoxScale = true;
    /// <summary>
    /// If true, and transformBoxScale is also true, modifies the center and Y bounds of each detected bounding box so that its
    /// bottom is at floor level (Y = 0) while keeping the other corners at the same place.
    /// </summary>
    [Tooltip("If true, and transformBoxScale is also true, modifies the center and Y bounds of each detected bounding box so that its " +
        "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.")]
    public bool transformBoxToTouchFloor = true;
    /// <summary>
    /// 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.
    /// </summary>
    [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. ")]
    [LabelOverride("Box Center Always On Floor")]
    public bool floorBBoxPosition = false;

    /// <summary>
    /// Display bounding boxes of objects that are actively being tracked by object tracking, where valid positions are known.
    /// </summary>
    [Space(5)]
    [Header("Filters")]
    [Tooltip("Display bounding boxes of objects that are actively being tracked by object tracking, where valid positions are known. ")]
    public bool showONTracked = true;
    /// <summary>
    /// Display bounding boxes of objects that were actively being tracked by object tracking, but that were lost very recently.
    /// </summary>
    [Tooltip("Display bounding boxes of objects that were actively being tracked by object tracking, but that were lost very recently.")]
    public bool showSEARCHINGTracked = false;
    /// <summary>
    /// Display bounding boxes of objects that are visible but not actively being tracked by object tracking (usually because object tracking is disabled in ZEDManager).
    /// </summary>
    [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).")]
    public bool showOFFTracked = false;

    /// <summary>
    /// How wide a bounding box has to be in order to be displayed. Use this to remove tiny bounding boxes from partially-occluded objects.
    /// (If you have this issue, it can also be helpful to set showSEARCHINGTracked to OFF.)
    /// </summary>
    [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" +
        "(If you have this issue, it can also be helpful to set showSEARCHINGTracked to OFF.)")]
    public float minimumWidthToDisplay = 0.3f;


    /// <summary>
    /// When a detected object is first given a box and assigned a color, we store it so that if the object
    /// disappears and appears again later, it's assigned the same color.
    /// This is also solvable by making the color a function of the ID number itself, but then you can get
    /// repeat colors under certain conditions.
    /// </summary>
    private Dictionary<int, Color> idColorDict = new Dictionary<int, Color>();

    /// <summary>
    /// Pre-instantiated bbox prefabs currently not in use.
    /// </summary>
    private Stack<GameObject> bboxPool = new Stack<GameObject>();

    /// <summary>
    /// All active GameObjects that were instantiated to the prefab and that currently represent a detected object.
    /// Key is the object's objectID.
    /// </summary>
    private Dictionary<int, GameObject> liveBBoxes = new Dictionary<int, GameObject>();

    /// <summary>
    /// Used to know which of the available colors will be assigned to the next bounding box to be used.
    /// </summary>
    private int nextColorIndex = 0;

    PersonManager PersonManager => personManagerLazy.Value;
    Lazy<PersonManager> personManagerLazy;

    private void Awake()
    {
        personManagerLazy = new(FindObjectOfType<PersonManager>());
    }

    // Use this for initialization
    void Start()
    {

    }

    private void Update()
    {
        Visualize3DBoundingBoxes(PersonManager.Persons);
    }

    private void ZedManager_OnObjectDetection(DetectionFrame objFrame)
    {
        //Visualize3DBoundingBoxes(objFrame);
        UpdateMinMaxCoordinates(objFrame);
    }

    private void UpdateMinMaxCoordinates(DetectionFrame objFrame)
    {
        List<DetectedObject> newobjects = objFrame.GetFilteredObjectList(showONTracked, showSEARCHINGTracked, showOFFTracked);
        foreach (var detectedObject in newobjects)
        {
            Bounds objbounds = detectedObject.Get3DWorldBounds();
            if (objbounds.size.x < minimumWidthToDisplay || objbounds.size == Vector3.zero) { }
            {
                Vector3 obj_position = detectedObject.Get3DWorldPosition();
                minX = Math.Min(minX, obj_position.x);
                maxX = Math.Max(maxX, obj_position.x);
                minY = Math.Min(minY, obj_position.y);
                maxY = Math.Max(maxY, obj_position.y);
                minZ = Math.Min(minZ, obj_position.z);
                maxZ = Math.Max(maxZ, obj_position.z);
            }
        }
    }

    float maxX = 0.9971107f;
    float minX = -0.7968294f;
    float maxY = int.MinValue;
    float minY = int.MaxValue;
    float maxZ = 2.373606f;
    float minZ = 0.4380694f;

    /// <summary>
    /// Given a frame of object detections, positions a GameObject to represent every visible object
    /// in that object's actual 3D location within the world.
    /// <para>Called from ZEDManager.OnObjectDetection each time there's a new detection frame available.</para>
    /// </summary>
    private void Visualize3DBoundingBoxes(IEnumerable<Person> persons)
    {
        //Get a list of all active IDs from last frame, and we'll remove each box that's visible this frame.
        //At the end, we'll clear the remaining boxes, as those are objects no longer visible to the ZED.
        List<int> activeids = liveBBoxes.Keys.ToList();

        foreach (var person in persons)
        {
            //GameObject bump = GameObject.Find($"Bump{count}");
            //Bounds objbounds = dobj.Get3DWorldBounds();
            //Make sure the object is big enough to count. We filter out very small boxes.
            //if (objbounds.size.x < minimumWidthToDisplay || objbounds.size == Vector3.zero) { }
            //{
            //Remove the ID from the list we'll use to clear no-longer-visible boxes.
            if (activeids.Contains(person.Id)) activeids.Remove(person.Id);

            //Get the box and update its distance value.
            GameObject bbox = GetBBoxForObject(person);

            //Move the box into position.
            //Vector3 obj_position = dobj.Get3DWorldPosition();

            //if (!ZEDSupportFunctions.IsVector3NaN(obj_position))
            //{
            bbox.transform.position = new Vector3(person.WorldPosition.x, 0, person.WorldPosition.y);

            if (floorBBoxPosition)
            {
                bbox.transform.position = new Vector3(bbox.transform.position.x, 0, bbox.transform.position.z);
            }

            var x = bbox.transform.position.x;

            var totalDistance = maxX - minX;
            var distanceFromStart = x - minX;
            var ratio = distanceFromStart / totalDistance;
            var newX = -12 + ratio * 24;

            var z = bbox.transform.position.z;

            totalDistance = maxZ - minZ;
            distanceFromStart = z - minZ;
            ratio = distanceFromStart / totalDistance;
            var newZ = -6 + ratio * 12;

            bbox.transform.position = new Vector3(newX, 0, newZ);
            //bbox.transform.rotation = dobj.Get3DWorldRotation(boxesFaceCamera); //Rotate them.

            //}

            //Transform the box if desired.
            //if (transformBoxScale)
            //{
            //    //We'll scale the object assuming that it's mesh is the default Unity cube, or something sized equally.
            //    if (transformBoxToTouchFloor)
            //    {
            //        Vector3 startscale = objbounds.size;
            //        float distfromfloor = bbox.transform.position.y - (objbounds.size.y / 2f);
            //        bbox.transform.localScale = new Vector3(objbounds.size.x, objbounds.size.y + distfromfloor, objbounds.size.z);
            //        Vector3 newpos = bbox.transform.position;
            //        newpos.y -= (distfromfloor / 2f);

            //        bbox.transform.position = newpos;

            //    }
            //    else
            //    {
            //        bbox.transform.localScale = objbounds.size;
            //    }
            //}

            //Now that we've adjusted position, tell the handler on the prefab to adjust distance display..
            BBox3DHandler boxhandler = bbox.GetComponent<BBox3DHandler>();
            //if (boxhandler)
            //{
            //    float disttobox = Vector3.Distance(dobj.detectingZEDManager.GetLeftCameraTransform().position, dobj.Get3DWorldPosition());
            //    boxhandler.SetDistance(disttobox);

            //    boxhandler.UpdateBoxUVScales();
            //    boxhandler.UpdateLabelScaleAndPosition();
            //}

            //DrawDebugBox(dobj);
            // }

            //Remove boxes for objects that the ZED can no longer see.
            foreach (int id in activeids)
            {
                ReturnBoxToPool(id, liveBBoxes[id]);
            }
        }
    }

    /// <summary>
    /// Returs the GameObject (instantiated from boundingBoxPrefab) that represents the provided DetectedObject.
    /// If none exists, it retrieves one from the pool (or instantiates a new one if none is available) and
    /// sets it up with the proper ID and colors.
    /// </summary>
    private GameObject GetBBoxForObject(Person dobj)
    {
        if (!liveBBoxes.ContainsKey(dobj.Id))
        {
            GameObject newbox = GetAvailableBBox();
            newbox.name = "Object #" + dobj.Id;

            BBox3DHandler boxhandler = newbox.GetComponent<BBox3DHandler>();

            Color col;
            if (idColorDict.ContainsKey(dobj.Id))
            {
                col = idColorDict[dobj.Id];
            }
            else
            {
                col = GetNextColor();
                idColorDict.Add(dobj.Id, col);
            }

            if (boxhandler)
            {
                boxhandler.SetColor(col);
                boxhandler.SetID(dobj.Id.ToString());
            }

            liveBBoxes[dobj.Id] = newbox;
            return newbox;
        }
        else return liveBBoxes[dobj.Id];
    }

    /// <summary>
    /// Gets an available GameObject (instantiated from boundingBoxPrefab) from the pool,
    /// or instantiates a new one if none are available.
    /// </summary>
    /// <returns></returns>
    private GameObject GetAvailableBBox()
    {
        if (bboxPool.Count == 0)
        {
            GameObject newbbox = Instantiate(boundingBoxPrefab);
            newbbox.transform.SetParent(transform, false);
            bboxPool.Push(newbbox);
        }

        GameObject bbox = bboxPool.Pop();
        bbox.SetActive(true);

        return bbox;
    }

    /// <summary>
    /// Disables a GameObject that was being used to represent an object (of the given id) and puts it back
    /// into the pool for later use.
    /// </summary>
    private void ReturnBoxToPool(int id, GameObject bbox)
    {
        bbox.SetActive(false);
        bbox.name = "Unused";

        bboxPool.Push(bbox);

        if (liveBBoxes.ContainsKey(id))
        {
            liveBBoxes.Remove(id);
        }
        else
        {
            Debug.LogError("Tried to remove object ID " + id + " from active bboxes, but it wasn't in the dictionary.");
        }
    }

    /// <summary>
    /// Returns a color from the boxColors list.
    /// Colors are returned sequentially in order of their appearance in that list.
    /// </summary>
    /// <returns></returns>
    private Color GetNextColor()
    {
        if (boxColors.Count == 0)
        {
            return new Color(.043f, .808f, .435f, 1);
        }

        if (nextColorIndex >= boxColors.Count)
        {
            nextColorIndex = 0;
        }

        Color returncol = boxColors[nextColorIndex];

        nextColorIndex++;


        return returncol;
    }

    private void OnDestroy()
    {
    }

    /// <summary>
    /// Draws a bounding box in the Scene window. Useful for debugging a 3D bbox's position relative to it.
    /// </summary>
    private void DrawDebugBox(DetectedObject dobj)
    {
        //Test bbox orientation.
        Transform camtrans = dobj.detectingZEDManager.GetLeftCameraTransform();
        Vector3[] corners = dobj.rawObjectData.worldBoundingBox;
        Vector3[] rotcorners = new Vector3[8];
        //Vector3[] corners3d = new Vector3[8];
        Vector3[] corners3d = dobj.Get3DWorldCorners();
        for (int i = 0; i < 8; i++)
        {
            Vector3 fixrot = camtrans.InverseTransformPoint(corners[i]);
            rotcorners[i] = fixrot;
        }


        Debug.DrawLine(corners3d[0], corners3d[1], Color.red);
        Debug.DrawLine(corners3d[1], corners3d[2], Color.red);
        Debug.DrawLine(corners3d[2], corners3d[3], Color.red);
        Debug.DrawLine(corners3d[3], corners3d[0], Color.red);

        Debug.DrawLine(corners3d[4], corners3d[5], Color.red);
        Debug.DrawLine(corners3d[5], corners3d[6], Color.red);
        Debug.DrawLine(corners3d[6], corners3d[7], Color.red);
        Debug.DrawLine(corners3d[7], corners3d[4], Color.red);

        Debug.DrawLine(corners3d[0], corners3d[4], Color.red);
        Debug.DrawLine(corners3d[1], corners3d[5], Color.red);
        Debug.DrawLine(corners3d[2], corners3d[6], Color.red);
        Debug.DrawLine(corners3d[3], corners3d[7], Color.red);
    }
}