public static void Run(string[] args)
{
var xefPath = @"C:\XEF\cam2_cal.xef";
var xef = new Xef(xefPath);
//Load computer vision (CV) color file
var colorCV = xef.LoadCvColorFrame(0);
colorCV.DrawAruco().ShowNoWait();
var cameraSpace = xef.LoadCVCameraSpace(2);
var(tx, markers) = Calibrator.Calibrate(colorCV, cameraSpace);
var pose = tx
.CameraSpaceToWorldTx
.ToMat();
var camSpaceTx = cameraSpace.Transform(pose)
.ToCamSpacePoints();
//Save as XYZRGB file (open in MeshLab to view)
XYZRGB.Export(camSpaceTx, colorCV.GetBRGABytes(), @"C:\XEF\cam2_cal.txt");
markers = markers.OrderByDescending(m => m.MaskSum.Val0).Take(4).ToList();
var markerPoints = new CvCameraSpace();
markers.ForEach(m => markerPoints.Add(m.KxCenter));
var txMarkers = markerPoints.Transform(pose);
XYZRGB.Export(txMarkers, new Scalar(255, 0, 0), @"C:\XEF\cam2_cal_markers.txt");
}
public static Point3f FindLocation(Point2f corn, CvCameraSpace cs)
{
Mat patch = new Mat();
Cv2.GetRectSubPix(cs, new Size(1, 1), corn, patch);
return(patch.At <Point3f>(0, 0));
}
public byte[] CameraPose()
{
//Create a defined registration pattern - in this case a cube
var cube = CoordinateDefinition.Microcube();
var yu2 = LatestYUVImage();
var colorCv = new CvColor(yu2);
//Find and draw (make sure it can be found)
var markers = Vision.FindAruco(colorCv);
if (!markers.Any())
{
return(PoseFormatter.PoseToBytes(new double[4, 4]));
} //zeros
//Calculate pose
var depth = LatestDepthImage();
CameraSpacePoint[] _3dImage = new CameraSpacePoint[KinectSettings.COLOR_PIXEL_COUNT];
KxBuffer.instance.coordinateMapper.MapColorFrameToCameraSpace(depth, _3dImage);
var cvCameraSpace = new CvCameraSpace(_3dImage);
var kxTransform = Vision.GetPoseFromImage(cube, cvCameraSpace, markers);
var pose = kxTransform.CameraPose;
return(PoseFormatter.PoseToBytes(pose));
}
public static Point3f FindCenter(Marker marker, CvCameraSpace cs)
{
//Isolate camera space to just this marker
var marker_3d = cs.SubMat(Cv2.BoundingRect(marker.Points));
//Mask out pixels with bad (infinite) data
var realMask = marker_3d.GetRealMask();
marker.MaskSum = realMask.Sum();
//Find the mean of all of the 3D points
var scalarCenter = marker_3d.Mean();
return(new Point3f((float)scalarCenter.Val0, (float)scalarCenter.Val1, (float)scalarCenter.Val2));
}
/// <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 (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);
}
