public override bool TryCompleteDetection(IReadOnlyList <Marker> markers, out Marker completedMarker)
{
if (markers.Count >= detector.RequiredObservations)
{
var averageMarker = CalculateAverageMarker(markers);
// Find a set of markers that are inliers (within a threshold of the average marker).
// This is used to reject spurious marker detections outside the norm to prevent them from polluting
// the final marker result.
var inliers = detector.CalculateInlierMarkerSet(markers, averageMarker, detector.MarkerInlierStandardDeviationThreshold);
if (inliers.Count >= detector.RequiredInlierCount)
{
// Recompute the average marker using only the set of inliers.
var averageInlierMarker = CalculateAverageMarker(inliers);
// Determine the standard deviation of the distance from the average marker and the angular
// delta from the average marker, and then see if that falls within the required threshold.
// If it does, we can stop. Otherwise, continue to gather samples until we get a set that
// does fall within the threshold.
double positionStandardDeviation, rotationStandardDeviation;
CalculateStandardDeviations(inliers, averageInlierMarker, out positionStandardDeviation, out rotationStandardDeviation);
if (positionStandardDeviation <= detector.MaximumPositionDistanceStandardDeviation && rotationStandardDeviation <= detector.MaximumRotationAngleStandardDeviation)
{
completedMarker = averageInlierMarker;
detector.LogMessagesAboutMarker("final", markers, inliers, averageMarker, averageInlierMarker);
return(true);
}
else
{
detector.LogMessagesAboutMarker("rejected", markers, inliers, averageMarker, averageInlierMarker);
}
}
}
completedMarker = null;
return(false);
}
public override bool TryCompleteDetection(IReadOnlyList <Marker> markers, out Marker completedMarker)
{
if (markers.Count >= detector.RequiredObservations)
{
completedMarker = CalculateAverageMarker(markers);
return(true);
}
else
{
completedMarker = null;
return(false);
}
}
/// <summary> /// Determines if the list of gathered markers is a representative sample, and if so computes the completed marker position. /// </summary> /// <param name="markers">A set of sampled positions and rotations of a physical marker.</param> /// <param name="completedMarker">A pose for the physical marker computed from the sampled positions.</param> /// <returns></returns> public abstract bool TryCompleteDetection(IReadOnlyList <Marker> markers, out Marker completedMarker);
private static bool IsMarkerInlier(Marker candidate, Marker averageMarker, double positionStandardDeviation, double rotationStandardDeviation, double markerInlierStandardDeviationThreshold)
{
return((candidate.Position - averageMarker.Position).magnitude < markerInlierStandardDeviationThreshold * positionStandardDeviation &&
Quaternion.Angle(candidate.Rotation, averageMarker.Rotation) < markerInlierStandardDeviationThreshold * rotationStandardDeviation);
}
private static void CalculateStandardDeviations(IReadOnlyList <Marker> allMarkers, Marker averageMarker, out double positionStandardDeviation, out double rotationStandardDeviation)
{
positionStandardDeviation = StandardDeviation(allMarkers, averageMarker, marker => (marker.Position - averageMarker.Position).magnitude);
rotationStandardDeviation = StandardDeviation(allMarkers, averageMarker, marker => Quaternion.Angle(marker.Rotation, averageMarker.Rotation));
}
private List <Marker> CalculateInlierMarkerSet(IReadOnlyList <Marker> allMarkers, Marker averageMarker, double markerInlierStandardDeviationThreshold)
{
double positionStandardDeviation, rotationStandardDeviation;
CalculateStandardDeviations(allMarkers, averageMarker, out positionStandardDeviation, out rotationStandardDeviation);
List <Marker> inliers = new List <Marker>(allMarkers.Count);
for (int i = 0; i < allMarkers.Count; i++)
{
if (IsMarkerInlier(allMarkers[i], averageMarker, positionStandardDeviation, rotationStandardDeviation, markerInlierStandardDeviationThreshold))
{
inliers.Add(allMarkers[i]);
}
else
{
DebugLog($"Marker Id: {allMarkers[i].Id} - Found an outlier: {allMarkers[i].Position} was {(allMarkers[i].Position - averageMarker.Position).magnitude} away and {Quaternion.Angle(allMarkers[i].Rotation, averageMarker.Rotation)} degrees from average. Standard deviation was pos:{positionStandardDeviation} and rot:{rotationStandardDeviation}");
}
}
return(inliers);
}
private void LogMessagesAboutMarker(string markerState, IReadOnlyList <Marker> allMarkers, IReadOnlyList <Marker> inlierMarkers, Marker averageMarker, Marker averageInlierMarker)
{
int markerId = allMarkers.Count > 0 ? allMarkers[0].Id : -1;
double positionStandardDeviation = StandardDeviation(allMarkers, averageMarker, marker => (marker.Position - averageMarker.Position).magnitude);
double rotationStandardDeviation = StandardDeviation(allMarkers, averageMarker, marker => Quaternion.Angle(marker.Rotation, averageMarker.Rotation));
double inlierPositionStandardDeviation = StandardDeviation(inlierMarkers, averageInlierMarker, marker => (marker.Position - averageInlierMarker.Position).magnitude);
double inlierRotationStandardDeviation = StandardDeviation(inlierMarkers, averageInlierMarker, marker => Quaternion.Angle(marker.Rotation, averageInlierMarker.Rotation));
DebugLog($"Marker Id: {markerId} - Calculated {markerState} marker position with {inlierMarkers.Count} markers out of {allMarkers.Count} available. Initial position standard deviation was {positionStandardDeviation} and rotation was {rotationStandardDeviation}. After outliers, position deviation was {inlierPositionStandardDeviation} and rotation was {inlierRotationStandardDeviation}. Final position was {averageInlierMarker.Position} which is {(averageInlierMarker.Position - averageMarker.Position).magnitude} away from original pose.");
}
