public static (KxTransform Transform, List <Marker> Markers) Calibrate(CvColor cvColor, CvCameraSpace cs)
{
//Define Board
var cube = CoordinateDefinition.Microcube();
//Look for Board
var markers = Vision.FindAruco(cvColor);
if (!markers.Any())
{
throw new Exception("No calibration pattern could be found in the image!");
}
//Calculate Camera Pose
return(Vision.GetPoseFromImage(cube, cs, markers), markers);
}
public static KxTransform GetPoseFromXef(string xefPath)
{
//Create a defined registration pattern - in this case a cube
var cube = CoordinateDefinition.Microcube();
//Find registration
var xef = new Xef(xefPath);
var colorCv = xef.LoadCvColorFrame(0);
//Find and draw (make sure it can be found)
var markers = Vision.FindAruco(colorCv);
//Vision.DrawAruco(colorCv).Show();
//Calculate pose
var _3dImage = xef.LoadCVCameraSpace(5);
var kxTransform = Vision.GetPoseFromImage(cube, _3dImage, markers);
return(kxTransform);
}
/// <summary>
/// Applies the input transform in reverse of the KxPosition to real to determine
/// the quality of the pose estimation.
/// </summary>
/// <param name="pose">the input pose 4x4 matrix</param>
/// <param name="def">the definition of the ARUCO pattern in real space</param>
/// <param name="markers">the markers found in the image</param>
/// <returns>a list of deltas of each transformed center vs the real center</returns>
public static List <float> ValidatePose(MatOfFloat pose, CoordinateDefinition def, List <Marker> markers)
{
List <float> deltas = new List <float>();
markers.ForEach(m =>
{
if (def.ContainsCode(m.Id))
{
var realPos = def.CenterDefinitions[m.Id];
var kPos = m.KxCenter;
var ptTx = pose.TransformPoint3f(kPos);
var delta = (ptTx - realPos).Magnitude();
if (!float.IsNaN(delta))
{
deltas.Add(delta);
}
}
});
return(deltas);
}
public static CoordinateDefinition Microcube()
{
var cubeDepth = 0.052f;
var markerFar = 0.0475f;
var markerClose = 0.0025f;
var width = 0.045f;
var bd = new CoordinateDefinition();
//Side 1 (1,2,3,4) - Proximal Cube Face
bd.CenterDefinitions.Add(0, new Point3f(0.025f, 0.025f, -cubeDepth));
bd.CenterDefinitions.Add(1, new Point3f(-0.025f, 0.025f, -cubeDepth));
bd.CenterDefinitions.Add(2, new Point3f(0.025f, -0.025f, -cubeDepth));
bd.CenterDefinitions.Add(3, new Point3f(-0.025f, -0.025f, -cubeDepth));
//Side 2 (4,5,6,7) - Right Cube Face
bd.CenterDefinitions.Add(4, new Point3f(-cubeDepth, 0.025f, -0.025f));
bd.CenterDefinitions.Add(5, new Point3f(-cubeDepth, 0.025f, 0.025f));
bd.CenterDefinitions.Add(6, new Point3f(-cubeDepth, -0.025f, -0.025f));
bd.CenterDefinitions.Add(7, new Point3f(-cubeDepth, -0.025f, 0.025f));
//Side 3 (8,9,10,11) - Distal Cube Face
bd.CenterDefinitions.Add(8, new Point3f(-0.025f, 0.025f, cubeDepth));
bd.CenterDefinitions.Add(9, new Point3f(0.025f, 0.025f, cubeDepth));
bd.CenterDefinitions.Add(10, new Point3f(-0.025f, -0.025f, cubeDepth));
bd.CenterDefinitions.Add(11, new Point3f(0.025f, -0.025f, cubeDepth));
//Side 4 (12,13,14,15) - Left Cube Face
bd.CenterDefinitions.Add(12, new Point3f(cubeDepth, 0.025f, 0.025f));
bd.CenterDefinitions.Add(13, new Point3f(cubeDepth, 0.025f, -0.025f));
bd.CenterDefinitions.Add(14, new Point3f(cubeDepth, -0.025f, 0.025f));
bd.CenterDefinitions.Add(15, new Point3f(cubeDepth, -0.025f, -0.025f));
//Side 5 - Top Cube Face
bd.CenterDefinitions.Add(16, new Point3f(0.025f, cubeDepth, 0.025f));
bd.CenterDefinitions.Add(17, new Point3f(-0.025f, cubeDepth, 0.025f));
bd.CenterDefinitions.Add(18, new Point3f(0.025f, cubeDepth, -0.025f));
bd.CenterDefinitions.Add(19, new Point3f(-0.025f, cubeDepth, -0.025f));
return(bd);
}
/// <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.");
}
public static void BuildRegistrationCube(string path, double cmWidth = 10.4)
{
var definition = CoordinateDefinition.Cube(11 * 2.5, cmWidth);
var dir = Path.GetDirectoryName(path);
var sides = new Dictionary <string, int>() //side, starting index id
{
{ "Side1", 0 },
{ "Side2", 4 },
{ "Side3", 8 },
{ "Side4", 12 },
{ "Side5", 16 },
{ "Side6", 20 },
};
var pixelsPerCM = 300 / 2.54;
//2 SQUARES WIDE, 2 SQUARES HIGH, 150 margin on width,
//100 margin on height
var paperPxWidth = pixelsPerCM * cmWidth; //11in - 1in margin *300px/in
var paperPxHeight = pixelsPerCM * cmWidth; //17in - 1in margin *300px/in
var markerPxWidth = (int)(pixelsPerCM * 4.5 / 10.4 * cmWidth);
var marginWidth = (int)((paperPxWidth - 2.0 * markerPxWidth) / 4);
var marginHeight = marginWidth;
var rows = 2;
var columns = 2;
var id = 0;
foreach (var side in sides)
{
Rect roi = new Rect(0, 0, markerPxWidth, markerPxWidth);
using (var outputImage = new Mat(new Size(paperPxWidth, paperPxHeight), MatType.CV_8UC1, Scalar.White))
{
for (var y = 0; y < rows; y++)
{
roi.Top = y * markerPxWidth + marginHeight * (y * 2 + 1);
for (var x = 0; x < columns; x++)
{
roi.Left = x * markerPxWidth + marginWidth * (x * 2 + 1);
using (var roiMat = new Mat(outputImage, roi))
using (var markerImage = new Mat())
using (var dict2 = CvAruco.GetPredefinedDictionary(PredefinedDictionaryName.Dict6X6_250))
{
CvAruco.DrawMarker(dict2, id++, markerPxWidth, markerImage, 1);
markerImage.CopyTo(roiMat);
}
}
}
var crossHairWidth = 1 * pixelsPerCM; // 1 cm
var crossHairColor = new Scalar(25, 25, 25);
Cv2.Line(outputImage, new Point((float)paperPxWidth / 2, (float)paperPxHeight / 2), new Point((float)paperPxWidth / 2, (float)(paperPxHeight / 2 - crossHairWidth)), crossHairColor);
Cv2.Line(outputImage, new Point((float)paperPxWidth / 2, (float)paperPxHeight / 2), new Point((float)paperPxWidth / 2, (float)(paperPxHeight / 2 + crossHairWidth)), crossHairColor);
Cv2.Line(outputImage, new Point((float)paperPxWidth / 2, (float)paperPxHeight / 2), new Point((float)(paperPxWidth / 2 - crossHairWidth), (float)(paperPxHeight / 2)), crossHairColor);
Cv2.Line(outputImage, new Point((float)paperPxWidth / 2, (float)paperPxHeight / 2), new Point((float)(paperPxWidth / 2 + crossHairWidth), (float)(paperPxHeight / 2)), crossHairColor);
Cv2.PutText(outputImage, side.Key, new Point((float)paperPxWidth / 2 - marginWidth / 1.3, (float)paperPxHeight), HersheyFonts.HersheyPlain, 1.5 / 10.4 * cmWidth, new Scalar(25, 25, 25), 1);
path = Path.Combine(dir, side.Key + ".png");
Cv2.ImWrite(path, outputImage);
}
}
}
public static CoordinateDefinition Cube(double cubeDepthCM, double squareWidthCM)
{
var pixelsPerCM = 300 / 2.54;
var cmWidth = squareWidthCM * 10.4 / 4.5;
var paperPxWidth = pixelsPerCM * cmWidth; //11in - 1in margin *300px/in
var paperPxHeight = pixelsPerCM * cmWidth; //17in - 1in margin *300px/in
var markerPxWidth = (int)(pixelsPerCM * 4.5 / 10.4 * cmWidth);
var marginWidthPx = (int)((paperPxWidth - 2.0 * markerPxWidth) / 4);
var marginHeight = marginWidthPx;
var markerFar = (float)((markerPxWidth + marginWidthPx) / pixelsPerCM);
var markerClose = (float)(marginWidthPx / pixelsPerCM);
float cubeDepth = (float)cubeDepthCM / 2.0f;
var width = (float)squareWidthCM;
var bd = new CoordinateDefinition();
//Side 1 (1,2,3,4) - Proximal Cube Face
bd.Add(0, new Point3f(markerFar, markerFar, -cubeDepth),
width, new Point3f(-1, 0, 0), new Point3f(0, -1, 0));
bd.Add(1, new Point3f(-markerClose, markerFar, -cubeDepth),
width, new Point3f(-1, 0, 0), new Point3f(0, -1, 0));
bd.Add(2, new Point3f(markerFar, -markerClose, -cubeDepth),
width, new Point3f(-1, 0, 0), new Point3f(0, -1, 0));
bd.Add(3, new Point3f(-markerClose, -markerClose, -cubeDepth),
width, new Point3f(-1, 0, 0), new Point3f(0, -1, 0));
//Side 2 (4,5,6,7) - Right Cube Face
bd.Add(4, new Point3f(-cubeDepth, markerFar, -markerFar),
width, new Point3f(0, 0, 1), new Point3f(0, -1, 0));
bd.Add(5, new Point3f(-cubeDepth, markerFar, markerClose),
width, new Point3f(0, 0, 1), new Point3f(0, -1, 0));
bd.Add(6, new Point3f(-cubeDepth, -markerClose, -markerFar),
width, new Point3f(0, 0, 1), new Point3f(0, -1, 0));
bd.Add(7, new Point3f(-cubeDepth, -markerClose, markerClose),
width, new Point3f(0, 0, 1), new Point3f(0, -1, 0));
//Side 3 (8,9,10,11) - Distal Cube Face
bd.Add(8, new Point3f(-markerFar, markerFar, cubeDepth),
width, new Point3f(1, 0, 0), new Point3f(0, -1, 0));
bd.Add(9, new Point3f(markerClose, markerFar, cubeDepth),
width, new Point3f(1, 0, 0), new Point3f(0, -1, 0));
bd.Add(10, new Point3f(-markerFar, -markerClose, cubeDepth),
width, new Point3f(1, 0, 0), new Point3f(0, -1, 0));
bd.Add(11, new Point3f(markerClose, -markerClose, cubeDepth),
width, new Point3f(1, 0, 0), new Point3f(0, -1, 0));
//Side 4 (12,13,14,15) - Left Cube Face
bd.Add(12, new Point3f(cubeDepth, markerFar, markerFar),
width, new Point3f(0, 0, -1), new Point3f(0, -1, 0));
bd.Add(13, new Point3f(cubeDepth, markerFar, -markerClose),
width, new Point3f(0, 0, -1), new Point3f(0, -1, 0));
bd.Add(14, new Point3f(cubeDepth, -markerClose, markerFar),
width, new Point3f(0, 0, -1), new Point3f(0, -1, 0));
bd.Add(15, new Point3f(cubeDepth, -markerClose, -markerClose),
width, new Point3f(0, 0, -1), new Point3f(0, -1, 0));
//Side 5 - Top Cube Face
bd.Add(16, new Point3f(markerFar, cubeDepth, markerFar),
width, new Point3f(-1, 0, 0), new Point3f(0, 0, -1));
bd.Add(17, new Point3f(-markerClose, cubeDepth, markerFar),
width, new Point3f(-1, 0, 0), new Point3f(0, 0, -1));
bd.Add(18, new Point3f(markerFar, cubeDepth, markerClose),
width, new Point3f(-1, 0, 0), new Point3f(0, 0, -1));
bd.Add(19, new Point3f(-markerClose, cubeDepth, markerClose),
width, new Point3f(-1, 0, 0), new Point3f(0, 0, -1));
return(bd);
}