123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256 |
- #if ZED_OPENCV_FOR_UNITY
- using System.Collections;
- using System.Collections.Generic;
- using UnityEngine;
- using OpenCVForUnity.CoreModule;
- using OpenCVForUnity.ArucoModule;
- using OpenCVForUnity.UnityUtils;
- using OpenCVForUnity.Calib3dModule;
- public class ZEDArUcoDetectionManager : MonoBehaviour
- {
-
-
-
-
- public ZEDToOpenCVRetriever imageRetriever;
-
-
-
-
- [Tooltip("Physical width of the printed ArUco markers.\r\n" +
- "Used to find the proper world position of the markers, so make sure this is accurate.")]
- public float markerWidthMeters = 0.2f;
-
-
-
-
-
-
- [Tooltip("Pre-defined OpenCV dictionary of marker images used to identify markers in the scene.\r\n" +
- "Note that images included in this project in the Resources folder are from the DICT_4X4 ones.")]
- public PreDefinedmarkerDictionary markerDictionary = PreDefinedmarkerDictionary.DICT_4X4_50;
-
-
-
-
-
- private static Dictionary<int, List<MarkerObject>> registeredMarkers = new Dictionary<int, List<MarkerObject>>();
- public delegate void MarkersDetectedEvent(Dictionary<int, List<sl.Pose>> detectedposes);
- public event MarkersDetectedEvent OnMarkersDetected;
-
-
-
-
-
- public static void RegisterMarker(MarkerObject marker)
- {
- if (!registeredMarkers.ContainsKey(marker.markerID))
- {
- registeredMarkers.Add(marker.markerID, new List<MarkerObject>());
- }
- List<MarkerObject> idlist = registeredMarkers[marker.markerID];
- if (!idlist.Contains(marker))
- {
- idlist.Add(marker);
- }
- else
- {
- Debug.LogError("Tried to register " + marker.gameObject.name + " as a new marker, but it was already registered.");
- }
- }
-
-
-
-
- public static void DeregisterMarker(MarkerObject marker)
- {
- if (registeredMarkers.ContainsKey(marker.markerID) && registeredMarkers[marker.markerID].Contains(marker))
- {
- registeredMarkers[marker.markerID].Remove(marker);
- }
- else
- {
- Debug.LogError("Tried to deregister " + marker.gameObject.name + " but it wasn't registered.");
- }
- }
-
- void Start()
- {
-
- if (!imageRetriever) imageRetriever = ZEDToOpenCVRetriever.GetInstance();
- imageRetriever.OnImageUpdated_LeftGrayscale += ImageUpdated;
- }
-
-
-
- private void ImageUpdated(Camera cam, Mat camMat, Mat iamgeMat)
- {
- Dictionary predict = Aruco.getPredefinedDictionary((int)markerDictionary);
-
- List<Mat> corners = new List<Mat>();
- Mat ids = new Mat();
- DetectorParameters detectparams = DetectorParameters.create();
- List<Mat> rejectedpoints = new List<Mat>();
-
- Aruco.detectMarkers(iamgeMat, predict, corners, ids, detectparams, rejectedpoints, camMat);
-
- Mat rotvectors = new Mat();
- Mat transvectors = new Mat();
-
- Aruco.estimatePoseSingleMarkers(corners, markerWidthMeters, camMat, new Mat(), rotvectors, transvectors);
-
-
-
-
-
-
-
-
- List<int> detectedIDs = new List<int>();
- for (int i = 0; i < ids.height(); i++)
- {
- int id = (int)ids.get(i, 0)[0];
- if (!detectedIDs.Contains(id)) detectedIDs.Add(id);
- }
-
- Dictionary<int, List<sl.Pose>> detectedWorldPoses = new Dictionary<int, List<sl.Pose>>();
-
- for(int index = 0; index < transvectors.rows(); index++)
- {
- int id = (int)ids.get(index, 0)[0];
- if (!registeredMarkers.ContainsKey(id) || registeredMarkers[id].Count == 0) continue;
- if (detectedIDs.Contains(id))
- {
-
- Vector3 localpos;
- localpos.x = (float)transvectors.get(index, 0)[0];
- localpos.y = -(float)transvectors.get(index, 0)[1];
- localpos.z = (float)transvectors.get(index, 0)[2];
- Vector3 worldpos = cam.transform.TransformPoint(localpos);
-
-
- double[] flip = rotvectors.get(index, 0);
- flip[1] = -flip[1];
- rotvectors.put(index, 0, flip);
-
-
- Mat rotmatrix = new Mat();
- Calib3d.Rodrigues(rotvectors.row(index), rotmatrix);
-
-
-
- Vector3 forward;
- forward.x = (float)rotmatrix.get(2, 0)[0];
- forward.y = (float)rotmatrix.get(2, 1)[0];
- forward.z = (float)rotmatrix.get(2, 2)[0];
- Vector3 up;
- up.x = (float)rotmatrix.get(1, 0)[0];
- up.y = (float)rotmatrix.get(1, 1)[0];
- up.z = (float)rotmatrix.get(1, 2)[0];
- Quaternion rot = Quaternion.LookRotation(forward, up);
-
- rot *= Quaternion.Euler(0, 0, 180);
-
- Quaternion worldrot = cam.transform.rotation * rot;
- if(!detectedWorldPoses.ContainsKey(id))
- {
- detectedWorldPoses.Add(id, new List<sl.Pose>());
- }
- detectedWorldPoses[id].Add(new sl.Pose() { translation = worldpos, rotation = worldrot });
- foreach (MarkerObject marker in registeredMarkers[id])
- {
- marker.MarkerDetectedSingle(worldpos, worldrot);
- }
- }
- }
-
- if (OnMarkersDetected != null) OnMarkersDetected.Invoke(detectedWorldPoses);
-
- foreach(int key in registeredMarkers.Keys)
- {
- if (detectedWorldPoses.ContainsKey(key))
- {
- foreach (MarkerObject marker in registeredMarkers[key])
- {
- marker.MarkerDetectedAll(detectedWorldPoses[key]);
- }
- }
- else
- {
- foreach (MarkerObject marker in registeredMarkers[key])
- {
- marker.MarkerNotDetected();
- }
- }
- }
- }
-
-
-
-
-
- public enum PreDefinedmarkerDictionary
- {
- DICT_4X4_50 = Aruco.DICT_4X4_50,
- DICT_4X4_100 = Aruco.DICT_4X4_100,
- DICT_4X4_250 = Aruco.DICT_4X4_250,
- DICT_4X4_1000 = Aruco.DICT_4X4_1000,
- DICT_5X5_50 = Aruco.DICT_5X5_50,
- DICT_5X5_100 = Aruco.DICT_5X5_100,
- DICT_5X5_250 = Aruco.DICT_5X5_250,
- DICT_5X5_1000 = Aruco.DICT_5X5_1000,
- DICT_6X6_50 = Aruco.DICT_6X6_50,
- DICT_6X6_100 = Aruco.DICT_6X6_100,
- DICT_6X6_250 = Aruco.DICT_6X6_250,
- DICT_6X6_1000 = Aruco.DICT_6X6_1000,
- DICT_7X7_50 = Aruco.DICT_7X7_50,
- DICT_7X7_100 = Aruco.DICT_7X7_100,
- DICT_7X7_250 = Aruco.DICT_7X7_250,
- DICT_7X7_1000 = Aruco.DICT_7X7_1000,
- DICT_ARUCO_ORIGINAL = Aruco.DICT_ARUCO_ORIGINAL,
- DICT_APRILTAG_16h5 = Aruco.DICT_APRILTAG_16h5,
- DICT_APRILTAG_25h9 = Aruco.DICT_APRILTAG_25h9,
- DICT_APRILTAG_36h10 = Aruco.DICT_APRILTAG_36h10,
- DICT_APRILTAG_36h11 = Aruco.DICT_APRILTAG_36h11
- }
- }
- #endif
|