protected virtual void CheckIfDetectedMarkers()
{
if (ids == null)
{
return;
}
int count = 0;
if (ids.Length > 0 && OnMarkersDetected != null)
{
for (int i = 0; i < ids.Length; i++)
{
if (!MarkerManager.IsMarkerRegistered(ids[i]))
{
count++;
}
}
if (count == ids.Length)
{
return;
}
OnMarkersDetected.Invoke(ids);
}
}
/// <summary>
/// Looks for markers in the most recent ZED image, and updates all registered MarkerObjects accordingly.
/// </summary>
private void ImageUpdated(Camera cam, Mat camMat, Mat iamgeMat)
{
Dictionary predict = Aruco.getPredefinedDictionary((int)markerDictionary); //Load the selected pre-defined dictionary.
//Create empty structures to hold the output of Aruco.detectMarkers.
List <Mat> corners = new List <Mat>();
Mat ids = new Mat();
DetectorParameters detectparams = DetectorParameters.create();
List <Mat> rejectedpoints = new List <Mat>(); //There is no overload for detectMarkers that will take camMat without this also.
//Call OpenCV to tell us which markers were detected, and give their 2D positions.
Aruco.detectMarkers(iamgeMat, predict, corners, ids, detectparams, rejectedpoints, camMat);
//Make matrices to hold the output rotations and translations of each detected marker.
Mat rotvectors = new Mat();
Mat transvectors = new Mat();
//Convert the 2D corner positions into a 3D pose for each detected marker.
Aruco.estimatePoseSingleMarkers(corners, markerWidthMeters, camMat, new Mat(), rotvectors, transvectors);
//Now we have ids, rotvectors and transvectors, which are all vertical arrays holding info about each detection:
// - ids: An Nx1 array (N = number of markers detected) where each slot is the ID of a detected marker in the dictionary.
// - rotvectors: An Nx3 array where each row is for an individual detection: The first row is the rotation of the marker
// listed in the first row of ids, etc. The columns are the X, Y and Z angles of that marker's rotation, BUT they are not
// directly usable in Unity because they're calculated very differetly. We'll deal with that soon.
// - transvectors: An Nx1 array like rotvectors with each row corresponding to a detected marker, with a double[3] array with the X, Y and Z positions.
// positions. These three values are usable in Unity - they're just relative to the camera, not the world, which is easy to fix.
//Convert matrix of IDs into a List, to simply things for those not familiar with using Matrices.
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);
}
}
//We'll go through every ID that's been registered into registered Markers, and see if we found any markers in the scene with that ID.
Dictionary <int, List <sl.Pose> > detectedWorldPoses = new Dictionary <int, List <sl.Pose> >(); //Key is marker ID, value is world space poses.
//foreach (int id in registeredMarkers.Keys)
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; //Don't waste time if the list is empty. Can happen if markers are added, then removed.
}
if (detectedIDs.Contains(id)) //At least one MarkerObject needs to be updated. Convert pose to world space and call MarkerDetected() on it.
{
//Translation is just pose relative to camera. But we need to flip Y because of OpenCV's different coordinate system.
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); //Convert from local to world space.
//Because of different coordinate frame, we need to flip the Y direction, which is pointing down instead of up.
//We need to do this before we calculate the 3x3 rotation matrix soon, as that makes it muuuch harder to work with.
double[] flip = rotvectors.get(index, 0);
flip[1] = -flip[1];
rotvectors.put(index, 0, flip);
//Convert this rotation vector to a 3x3 matrix, which will hold values we can use in Unity.
Mat rotmatrix = new Mat();
Calib3d.Rodrigues(rotvectors.row(index), rotmatrix);
//This new 3x3 matrix holds a vector pointing right in the first column, a vector pointing up in the second,
//and a vector pointing forward in the third column. Rows 0, 1 and 2 are the X, Y and Z values of each vector.
//We'll grab the forward and up vectors, which we can put into Quaternion.LookRotation() to get a representative Quaternion.
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);
//Compensate for flip on Z axis.
rot *= Quaternion.Euler(0, 0, 180);
//Convert from local space to world space by multiplying the camera's world rotation with it.
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);
}
}
}
//Call the event that gives all marker world poses, if any listeners.
if (OnMarkersDetected != null)
{
OnMarkersDetected.Invoke(detectedWorldPoses);
}
//foreach (int detectedid in detectedWorldPoses.Keys)
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();
}
}
}
}
