From dbb009ee3ba0a3d7544314d988f9cf5c7c34233e Mon Sep 17 00:00:00 2001 From: bmad4ever Date: Tue, 19 Nov 2019 20:41:23 +0000 Subject: [PATCH] Fixed race conditions on MarkerTracker Related to the following issue: https://github.com/NormandErwan/ArucoUnity/issues/28 --- .../Objects/Trackers/ArucoMarkerTracker.cs | 359 ++++++++++-------- 1 file changed, 196 insertions(+), 163 deletions(-) diff --git a/Assets/ArucoUnity/Scripts/Objects/Trackers/ArucoMarkerTracker.cs b/Assets/ArucoUnity/Scripts/Objects/Trackers/ArucoMarkerTracker.cs index 1b5ec00f..861918e9 100644 --- a/Assets/ArucoUnity/Scripts/Objects/Trackers/ArucoMarkerTracker.cs +++ b/Assets/ArucoUnity/Scripts/Objects/Trackers/ArucoMarkerTracker.cs @@ -1,206 +1,239 @@ -using ArucoUnity.Plugin; +using ArucoUnity.Plugin; using System.Collections.Generic; using UnityEngine; namespace ArucoUnity.Objects.Trackers { - public class ArucoMarkerTracker : ArucoObjectTracker - { - // Constants + public class ArucoMarkerTracker : ArucoObjectTracker + { + // Constants - protected const float estimatePoseMarkerLength = 1f; - protected readonly Color rejectedMarkerCandidatesColor = new Color(100, 0, 255); + protected const float estimatePoseMarkerLength = 1f; + protected readonly Color rejectedMarkerCandidatesColor = new Color(100, 0, 255); - // Properties + // Properties - public Dictionary[] DetectedMarkers { get; protected internal set; } + public Dictionary[] DetectedMarkers { get; protected internal set; } - /// - /// Vector of the detected marker corners on each . Updated by . - /// - public Dictionary[] MarkerCorners { get; protected internal set; } + /// + /// Vector of the detected marker corners on each . Updated by . + /// + public Dictionary[] MarkerCorners { get; protected internal set; } - /// - /// Vector of identifiers of the detected markers on each . Updated by . - /// - public Dictionary[] MarkerIds { get; protected internal set; } + /// + /// Vector of identifiers of the detected markers on each . Updated by . + /// + public Dictionary[] MarkerIds { get; protected internal set; } - /// - /// Vector of the corners with not a correct identification on each . Updated by . - /// - public Dictionary[] RejectedCandidateCorners { get; protected internal set; } + /// + /// Vector of the corners with not a correct identification on each . Updated by . + /// + public Dictionary[] RejectedCandidateCorners { get; protected internal set; } - /// - /// Vector of rotation vectors of the detected markers on each . - /// - public Dictionary[] MarkerRvecs { get; protected internal set; } + /// + /// Vector of rotation vectors of the detected markers on each . + /// + public Dictionary[] MarkerRvecs { get; protected internal set; } - /// - /// Vector of translation vectors of the detected markers on each . - /// - public Dictionary[] MarkerTvecs { get; protected internal set; } + /// + /// Vector of translation vectors of the detected markers on each . + /// + public Dictionary[] MarkerTvecs { get; protected internal set; } - // ArucoObjectsController related methods + /// + /// Combines the data from MarkerIds, MarkerTvecs, and MarkerRvecs. + /// It is locked on EstimateTransforms and Updatetransforms methods to avoid race conditions. + /// + public Dictionary>[] MarkersData { get; protected internal set; } - protected override void ArucoObjectsController_DictionaryAdded(Aruco.Dictionary dictionary) - { - for (int cameraId = 0; cameraId < arucoCamera.CameraNumber; cameraId++) - { - MarkerCorners[cameraId].Add(dictionary, new Std.VectorVectorPoint2f()); - MarkerIds[cameraId].Add(dictionary, new Std.VectorInt()); - RejectedCandidateCorners[cameraId].Add(dictionary, new Std.VectorVectorPoint2f()); - MarkerRvecs[cameraId].Add(dictionary, new Std.VectorVec3d()); - MarkerTvecs[cameraId].Add(dictionary, new Std.VectorVec3d()); - DetectedMarkers[cameraId].Add(dictionary, 0); - } - } - protected override void ArucoObjectsController_DictionaryRemoved(Aruco.Dictionary dictionary) - { - for (int cameraId = 0; cameraId < arucoCamera.CameraNumber; cameraId++) - { - MarkerCorners[cameraId].Remove(dictionary); - MarkerIds[cameraId].Remove(dictionary); - RejectedCandidateCorners[cameraId].Remove(dictionary); - MarkerRvecs[cameraId].Remove(dictionary); - MarkerTvecs[cameraId].Remove(dictionary); - DetectedMarkers[cameraId].Remove(dictionary); - } - } + // ArucoObjectsController related methods - // ArucoObjectTracker methods + protected override void ArucoObjectsController_DictionaryAdded(Aruco.Dictionary dictionary) + { + for (int cameraId = 0; cameraId < arucoCamera.CameraNumber; cameraId++) + { + MarkerCorners[cameraId].Add(dictionary, new Std.VectorVectorPoint2f()); + MarkerIds[cameraId].Add(dictionary, new Std.VectorInt()); + RejectedCandidateCorners[cameraId].Add(dictionary, new Std.VectorVectorPoint2f()); + MarkerRvecs[cameraId].Add(dictionary, new Std.VectorVec3d()); + MarkerTvecs[cameraId].Add(dictionary, new Std.VectorVec3d()); + DetectedMarkers[cameraId].Add(dictionary, 0); + MarkersData[cameraId].Add(dictionary, new List<(int markerID, Vector3 position, Quaternion rotation)>()); + } + } - public override void Activate(IArucoObjectsTracker arucoTracker) - { - base.Activate(arucoTracker); - - // Initialize the properties and the ArUco objects - MarkerCorners = new Dictionary[arucoCamera.CameraNumber]; - MarkerIds = new Dictionary[arucoCamera.CameraNumber]; - RejectedCandidateCorners = new Dictionary[arucoCamera.CameraNumber]; - MarkerRvecs = new Dictionary[arucoCamera.CameraNumber]; - MarkerTvecs = new Dictionary[arucoCamera.CameraNumber]; - DetectedMarkers = new Dictionary[arucoCamera.CameraNumber]; - - for (int cameraId = 0; cameraId < arucoCamera.CameraNumber; cameraId++) - { - MarkerCorners[cameraId] = new Dictionary(); - MarkerIds[cameraId] = new Dictionary(); - RejectedCandidateCorners[cameraId] = new Dictionary(); - MarkerRvecs[cameraId] = new Dictionary(); - MarkerTvecs[cameraId] = new Dictionary(); - DetectedMarkers[cameraId] = new Dictionary(); - - foreach (var arucoObjectDictionary in arucoTracker.ArucoObjects) + protected override void ArucoObjectsController_DictionaryRemoved(Aruco.Dictionary dictionary) { - Aruco.Dictionary dictionary = arucoObjectDictionary.Key; - - MarkerCorners[cameraId].Add(dictionary, new Std.VectorVectorPoint2f()); - MarkerIds[cameraId].Add(dictionary, new Std.VectorInt()); - RejectedCandidateCorners[cameraId].Add(dictionary, new Std.VectorVectorPoint2f()); - MarkerRvecs[cameraId].Add(dictionary, new Std.VectorVec3d()); - MarkerTvecs[cameraId].Add(dictionary, new Std.VectorVec3d()); - DetectedMarkers[cameraId].Add(dictionary, 0); + for (int cameraId = 0; cameraId < arucoCamera.CameraNumber; cameraId++) + { + MarkerCorners[cameraId].Remove(dictionary); + MarkerIds[cameraId].Remove(dictionary); + RejectedCandidateCorners[cameraId].Remove(dictionary); + MarkerRvecs[cameraId].Remove(dictionary); + MarkerTvecs[cameraId].Remove(dictionary); + DetectedMarkers[cameraId].Remove(dictionary); + MarkersData[cameraId].Remove(dictionary); + } } - } - } - public override void Deactivate() - { - base.Deactivate(); - - MarkerCorners = null; - MarkerIds = null; - RejectedCandidateCorners = null; - MarkerRvecs = null; - MarkerTvecs = null; - DetectedMarkers = null; - } + // ArucoObjectTracker methods - public override void Detect(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image) - { - base.Detect(cameraId, dictionary, image); + public override void Activate(IArucoObjectsTracker arucoTracker) + { + base.Activate(arucoTracker); + + // Initialize the properties and the ArUco objects + MarkerCorners = new Dictionary[arucoCamera.CameraNumber]; + MarkerIds = new Dictionary[arucoCamera.CameraNumber]; + RejectedCandidateCorners = new Dictionary[arucoCamera.CameraNumber]; + MarkerRvecs = new Dictionary[arucoCamera.CameraNumber]; + MarkerTvecs = new Dictionary[arucoCamera.CameraNumber]; + DetectedMarkers = new Dictionary[arucoCamera.CameraNumber]; + MarkersData = new Dictionary>[arucoCamera.CameraNumber]; + + for (int cameraId = 0; cameraId < arucoCamera.CameraNumber; cameraId++) + { + MarkerCorners[cameraId] = new Dictionary(); + MarkerIds[cameraId] = new Dictionary(); + RejectedCandidateCorners[cameraId] = new Dictionary(); + MarkerRvecs[cameraId] = new Dictionary(); + MarkerTvecs[cameraId] = new Dictionary(); + DetectedMarkers[cameraId] = new Dictionary(); + MarkersData[cameraId] = new Dictionary>(); + + foreach (var arucoObjectDictionary in arucoTracker.ArucoObjects) + { + Aruco.Dictionary dictionary = arucoObjectDictionary.Key; + + MarkerCorners[cameraId].Add(dictionary, new Std.VectorVectorPoint2f()); + MarkerIds[cameraId].Add(dictionary, new Std.VectorInt()); + RejectedCandidateCorners[cameraId].Add(dictionary, new Std.VectorVectorPoint2f()); + MarkerRvecs[cameraId].Add(dictionary, new Std.VectorVec3d()); + MarkerTvecs[cameraId].Add(dictionary, new Std.VectorVec3d()); + DetectedMarkers[cameraId].Add(dictionary, 0); + MarkersData[cameraId].Add(dictionary, new List<(int markerID, Vector3 position, Quaternion rotation)>()); + } + } + } - Std.VectorVectorPoint2f markerCorners, rejectedCandidateCorners; - Std.VectorInt markerIds; + public override void Deactivate() + { + base.Deactivate(); + + MarkerCorners = null; + MarkerIds = null; + RejectedCandidateCorners = null; + MarkerRvecs = null; + MarkerTvecs = null; + DetectedMarkers = null; + MarkersData = null; + } - Aruco.DetectMarkers(image, dictionary, out markerCorners, out markerIds, arucoTracker.DetectorParameters, out rejectedCandidateCorners); + public override void Detect(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image) + { + base.Detect(cameraId, dictionary, image); - DetectedMarkers[cameraId][dictionary] = (int)markerIds.Size(); - MarkerCorners[cameraId][dictionary] = markerCorners; - MarkerIds[cameraId][dictionary] = markerIds; - RejectedCandidateCorners[cameraId][dictionary] = rejectedCandidateCorners; - } + Std.VectorVectorPoint2f markerCorners, rejectedCandidateCorners; + Std.VectorInt markerIds; - public override void Draw(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image) - { - base.Draw(cameraId, dictionary, image); + Aruco.DetectMarkers(image, dictionary, out markerCorners, out markerIds, arucoTracker.DetectorParameters, out rejectedCandidateCorners); - if (DetectedMarkers[cameraId][dictionary] > 0) - { - // Draw all the detected markers - if (arucoTracker.DrawDetectedMarkers) - { - Aruco.DrawDetectedMarkers(image, MarkerCorners[cameraId][dictionary], MarkerIds[cameraId][dictionary]); + DetectedMarkers[cameraId][dictionary] = (int)markerIds.Size(); + MarkerCorners[cameraId][dictionary] = markerCorners; + MarkerIds[cameraId][dictionary] = markerIds; + RejectedCandidateCorners[cameraId][dictionary] = rejectedCandidateCorners; } - // Draw axes of detected tracked markers - if (arucoTracker.DrawAxes && arucoCameraUndistortion != null && MarkerRvecs[cameraId][dictionary] != null) + public override void Draw(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image) { - for (uint i = 0; i < DetectedMarkers[cameraId][dictionary]; i++) - { - ArucoObject foundArucoObject; - int detectedMarkerHashCode = ArucoMarker.GetArucoHashCode(MarkerIds[cameraId][dictionary].At(i)); - if (arucoTracker.ArucoObjects[dictionary].TryGetValue(detectedMarkerHashCode, out foundArucoObject)) + base.Draw(cameraId, dictionary, image); + + if (DetectedMarkers[cameraId][dictionary] > 0) + { + // Draw all the detected markers + if (arucoTracker.DrawDetectedMarkers) + { + Aruco.DrawDetectedMarkers(image, MarkerCorners[cameraId][dictionary], MarkerIds[cameraId][dictionary]); + } + + // Draw axes of detected tracked markers + if (arucoTracker.DrawAxes && arucoCameraUndistortion != null && MarkerRvecs[cameraId][dictionary] != null) + { + for (uint i = 0; i < DetectedMarkers[cameraId][dictionary]; i++) + { + ArucoObject foundArucoObject; + int detectedMarkerHashCode = ArucoMarker.GetArucoHashCode(MarkerIds[cameraId][dictionary].At(i)); + if (arucoTracker.ArucoObjects[dictionary].TryGetValue(detectedMarkerHashCode, out foundArucoObject)) + { + Aruco.DrawAxis(image, arucoCameraUndistortion.RectifiedCameraMatrices[cameraId], arucoCameraUndistortion.UndistortedDistCoeffs[cameraId], + MarkerRvecs[cameraId][dictionary].At(i), MarkerTvecs[cameraId][dictionary].At(i), estimatePoseMarkerLength); + } + } + } + } + + // Draw the rejected marker candidates + if (arucoTracker.DrawRejectedCandidates && RejectedCandidateCorners[cameraId][dictionary].Size() > 0) { - Aruco.DrawAxis(image, arucoCameraUndistortion.RectifiedCameraMatrices[cameraId], arucoCameraUndistortion.UndistortedDistCoeffs[cameraId], - MarkerRvecs[cameraId][dictionary].At(i), MarkerTvecs[cameraId][dictionary].At(i), estimatePoseMarkerLength); + Aruco.DrawDetectedMarkers(image, RejectedCandidateCorners[cameraId][dictionary]); } - } } - } - // Draw the rejected marker candidates - if (arucoTracker.DrawRejectedCandidates && RejectedCandidateCorners[cameraId][dictionary].Size() > 0) - { - Aruco.DrawDetectedMarkers(image, RejectedCandidateCorners[cameraId][dictionary]); - } - } + public override void EstimateTransforms(int cameraId, Aruco.Dictionary dictionary) + { + base.EstimateTransforms(cameraId, dictionary); - public override void EstimateTransforms(int cameraId, Aruco.Dictionary dictionary) - { - base.EstimateTransforms(cameraId, dictionary); + Std.VectorVec3d rvecs = null, tvecs = null; - Std.VectorVec3d rvecs = null, tvecs = null; + //Detect markers + if (DetectedMarkers[cameraId][dictionary] > 0 && arucoCameraUndistortion != null) + { + Aruco.EstimatePoseSingleMarkers(MarkerCorners[cameraId][dictionary], estimatePoseMarkerLength, arucoCameraUndistortion.RectifiedCameraMatrices[cameraId], + arucoCameraUndistortion.UndistortedDistCoeffs[cameraId], out rvecs, out tvecs); + } - if (DetectedMarkers[cameraId][dictionary] > 0 && arucoCameraUndistortion != null) - { - Aruco.EstimatePoseSingleMarkers(MarkerCorners[cameraId][dictionary], estimatePoseMarkerLength, arucoCameraUndistortion.RectifiedCameraMatrices[cameraId], - arucoCameraUndistortion.UndistortedDistCoeffs[cameraId], out rvecs, out tvecs); - } + MarkerRvecs[cameraId][dictionary] = rvecs; + MarkerTvecs[cameraId][dictionary] = tvecs; - MarkerRvecs[cameraId][dictionary] = rvecs; - MarkerTvecs[cameraId][dictionary] = tvecs; - } + //Update list of markers to update + lock (MarkersData[cameraId][dictionary]) + { + MarkersData[cameraId][dictionary].Clear(); + + for (uint i = 0; i < DetectedMarkers[cameraId][dictionary]; i++) + MarkersData[cameraId][dictionary].Add( + ( + MarkerIds[cameraId][dictionary].At(i), + MarkerTvecs[cameraId][dictionary].At(i).ToPosition(), + MarkerRvecs[cameraId][dictionary].At(i).ToRotation() + ) + ); + } - public override void UpdateTransforms(int cameraId, Aruco.Dictionary dictionary) - { - base.UpdateTransforms(cameraId, dictionary); + } - if (MarkerRvecs[cameraId][dictionary] != null) - { - for (uint i = 0; i < DetectedMarkers[cameraId][dictionary]; i++) + public override void UpdateTransforms(int cameraId, Aruco.Dictionary dictionary) { - ArucoObject foundArucoObject; - int detectedMarkerHashCode = ArucoMarker.GetArucoHashCode(MarkerIds[cameraId][dictionary].At(i)); - if (arucoTracker.ArucoObjects[dictionary].TryGetValue(detectedMarkerHashCode, out foundArucoObject)) - { - var localPosition = MarkerTvecs[cameraId][dictionary].At(i).ToPosition() * foundArucoObject.MarkerSideLength / estimatePoseMarkerLength; - arucoCameraDisplay.PlaceArucoObject(foundArucoObject.transform, cameraId, localPosition, - MarkerRvecs[cameraId][dictionary].At(i).ToRotation()); - } + base.UpdateTransforms(cameraId, dictionary); + + if (MarkersData[cameraId][dictionary] != null) + lock (MarkersData[cameraId][dictionary]) + { + for (uint i = 0; i < MarkersData[cameraId][dictionary].Count; i++) + { + int index = (int)i; + + ArucoObject foundArucoObject; + int detectedMarkerHashCode = ArucoMarker.GetArucoHashCode(MarkersData[cameraId][dictionary][index].markerID); + if (arucoTracker.ArucoObjects[dictionary].TryGetValue(detectedMarkerHashCode, out foundArucoObject)) + { + var localPosition = MarkersData[cameraId][dictionary][index].position * foundArucoObject.MarkerSideLength / estimatePoseMarkerLength; + arucoCameraDisplay.PlaceArucoObject(foundArucoObject.transform, cameraId, localPosition, + MarkersData[cameraId][dictionary][index].rotation); + } + } + } } - } } - } -} \ No newline at end of file +}