private static MatOfPoint3f Subtract(MatOfPoint3f orig, MatOfPoint3f aCentroidMatrix)
{
MatOfPoint3f answer = new MatOfPoint3f();
for (int i = 0; i < orig.Rows; i++)
{
answer.Add(orig.ElementAt(i) - aCentroidMatrix.ElementAt(i));
}
return(answer);
}
public static MatOfPoint3f GetCentroid(MatOfPoint3f orig)
{
int i = 0;
var centroid = new Point3f();
foreach (var p in orig)
{
centroid += p;
i++;
}
var point = new Point3f(centroid.X / i, centroid.Y / i, centroid.Z / i);
var mat = new MatOfPoint3f();
mat.Add(point);
return(mat);
}
/// <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 MatOfFloat TransformBetween(MatOfPoint3f origPoint3f, MatOfPoint3f destPoint3f)
{
int rowCountA = origPoint3f.Rows;
int rowCountB = destPoint3f.Rows;
if (rowCountA != rowCountB)
{
throw new Exception("Data must be paired. The number of rows should be equal in both input matrices!");
}
var orig = ConvertToFC1(origPoint3f);
var dest = ConvertToFC1(destPoint3f);
//OCV
var origCentroid = GetCentroid(origPoint3f);
var destCentroid = GetCentroid(destPoint3f);
var aCentroidMatrix = new MatOfPoint3f();
for (int rep = 0; rep < rowCountA; rep++)
{
aCentroidMatrix.Add(origCentroid);
}
var bCentroidMatrix = new MatOfPoint3f();
for (int rep = 0; rep < rowCountB; rep++)
{
bCentroidMatrix.Add(destCentroid);
}
//OCV
var step1 = Subtract(origPoint3f, aCentroidMatrix);
var step2 = Subtract(destPoint3f, bCentroidMatrix);
Mat reshapeStep1 = Reshape(step1);
Mat reshapeStep2 = Reshape(step2);
var h = (reshapeStep1.Transpose() * reshapeStep2).ToMat();
Mat w = new Mat();
Mat u = new Mat();
Mat vt = new Mat();
Cv2.SVDecomp(h, w, u, vt);
Mat V = vt.Transpose();
var r = (V * (u.Transpose())).ToMat();
var first = r * (-1);
var second = first.ToMat() * origCentroid;
var t = (((r * (-1)) * (Reshape(origCentroid).Transpose())) + Reshape(destCentroid).Transpose()).ToMat();
//Set translation component
var tvals = new float[3];
t.GetArray(0, 0, tvals);
var mat = new MatOfFloat(4, 4);
mat.Set(0, 3, tvals[0]);
mat.Set(1, 3, tvals[1]);
mat.Set(2, 3, tvals[2]);
mat.Set(3, 3, 1.0f);
////Set rotation componen
for (int row = 0; row < 3; row++)
{
var vals = new float[3];
r.GetArray(row, 0, vals);
mat.SetArray(row, 0, vals);
}
return(mat);
}
