/// <summary>
/// Calculates the camera pose (Kinect space to real space transform) based on coordinate definition
/// and detected markers
/// </summary>
/// <param name="def">the definition of the visible coordinates</param>
/// <param name="_3dImage">the 3d points mapped to Kinect color coordinates</param>
/// <param name="markers">the detected markers in the color image</param>
/// <returns>a 4x4 matrix of the camera pose</returns>
public static KxTransform GetPoseFromImage(CoordinateDefinition def, CvCameraSpace cvcs, List <Marker> markers)
{
MatOfPoint3f sourcePts = new MatOfPoint3f();
MatOfPoint3f destPts = new MatOfPoint3f();
if (markers != null)
{
//For each marker found, look up Kinect position (2D -> 3D), find corresponding real position
//Add KPos and RealPos to two arrays to calculate the transform between
markers.ForEach(m =>
{
if (def.ContainsCode(m.Id))
{
m.KxCenter = MarkerProcessor.FindCenter(m, cvcs);
}
});
//Todo: Take N best markers (highest mask sum means better 3d data)
var ordered = markers.OrderByDescending(m => m.MaskSum.Val0).ToList();
MatOfFloat tx = null;
var lastDelta = float.MinValue;
double lastStd = float.MinValue;
foreach (var m in ordered)
{
//ADD CENTER
var realPos = def.CenterDefinitions[m.Id];
if (!float.IsInfinity(m.KxCenter.X))
{
//Source is Kinect position
sourcePts.Add(m.KxCenter);
//Destination is actual physical location
destPts.Add(realPos);
_logger.Info($"Adding point {m.Id} : {m.KxCenter} =>");
_logger.Info($"{realPos}");
}
//ADD CORNERS
for (int p = 0; p < m.Points.Length; p++)
{
var corn = m.Points[p];
var kxCornerPosition = MarkerProcessor.FindLocation(corn, cvcs);
if (!float.IsInfinity(kxCornerPosition.X))
{
//Source is Kinect position
sourcePts.Add(kxCornerPosition);
//Destination is actual physical location
destPts.Add(def.CornerDefinitions[m.Id][p]);
_logger.Info($"Adding point {m.Id} : {kxCornerPosition} =>");
_logger.Info($"{def.CornerDefinitions[m.Id][p]}");
if (sourcePts.Total() >= 3)
{
_logger.Info($"Using {sourcePts.Count} markers to find pose...");
var newTx = Transform.TransformBetween(sourcePts, destPts);
var deltas = ValidatePose(newTx, def, markers);
var avgDelta = deltas.Average();
var std = deltas.StdDev();
//If it is better update
if (tx == null || Math.Abs(avgDelta) < Math.Abs(lastDelta))
{
lastDelta = avgDelta;
lastStd = std;
tx = newTx;
}
else
{
break;
}
}
}
}
if (sourcePts.Total() >= 15)
{
_logger.Info($"Using {sourcePts.Count} markers to find pose...");
var newTx = Transform.TransformBetween(sourcePts, destPts);
var deltas = ValidatePose(newTx, def, markers);
var avgDelta = deltas.Average();
var std = deltas.StdDev();
//If it is better update
if (tx == null || Math.Abs(avgDelta) < Math.Abs(lastDelta))
{
lastDelta = avgDelta;
lastStd = std;
tx = newTx;
}
else
{
break;
}
}
}
_logger.Info($"Pose calculated with average delta of : ");
_logger.Info($"{(lastDelta * 1000).ToString("N3")} ± {(lastStd * 1000).ToString("N3")} mm");
//Need to flip Z to get into DICOM coordinates
if (!sourcePts.Any() || !destPts.Any())
{
throw new Exception("No points to transform!");
}
var kxTx = new KxTransform(tx.To2DArray());
return(kxTx);
}
throw new ArgumentException("Markers cannot be null.");
}