public void Test()
{
//AR用カメラパラメタファイルをロード
NyARParam ap = NyARParam.createFromARParamFile(new StreamReader(camera_file));
ap.changeScreenSize(320, 240);
//試験イメージの読み出し(320x240 RGBのRAWデータ)
StreamReader sr = new StreamReader(data_file);
BinaryReader bs = new BinaryReader(sr.BaseStream);
byte[] raw = bs.ReadBytes(320 * 240 * 3);
NyARRgbRaster ra = new NyARRgbRaster(320, 240, NyARBufferType.BYTE1D_R8G8B8_24, false);
ra.wrapBuffer(raw);
MarkerProcessor pr = new MarkerProcessor(ap, ra.getBufferType());
pr.detectMarker(ra);
Console.WriteLine(pr.transmat.m00 + "," + pr.transmat.m01 + "," + pr.transmat.m02 + "," + pr.transmat.m03);
Console.WriteLine(pr.transmat.m10 + "," + pr.transmat.m11 + "," + pr.transmat.m12 + "," + pr.transmat.m13);
Console.WriteLine(pr.transmat.m20 + "," + pr.transmat.m21 + "," + pr.transmat.m22 + "," + pr.transmat.m23);
Console.WriteLine(pr.transmat.m30 + "," + pr.transmat.m31 + "," + pr.transmat.m32 + "," + pr.transmat.m33);
Console.WriteLine(pr.current_id);
return;
}
public static double[] GetRotationAngles(UserInputForAngleCalculation[] userInputForAngleCalculations)
{
var markerProcessor = new MarkerProcessor();
var angles = new double[userInputForAngleCalculations.Length];
for (var ctr = 0; ctr < userInputForAngleCalculations.Length; ctr++)
{
var input = userInputForAngleCalculations[ctr];
angles[ctr] = markerProcessor.GetNextAngleOfRotation(
input.LeftOfCenterFirstMarkerXPos,
input.FirstRightOfFirstMarkerXPos,
input.LeftEdgePixelXPos,
input.RightEdgePixelXPos);
}
return(angles);
}
public void LeftAndRightMarkerPositionsTest()
{
var backgroundColor = TestHelper.GetBackgroundColor();
var image = TestHelper.GetStrippedTestImageForPerfectlyVerticalBase(backgroundColor);
const string outputFile = @"\LeftAndRightMarkerPositionsTest.png";
var xPositions = MarkerProcessor.GetLeftAndRightMarkerPositions(image, 229, 581, 500, new MarkerProcessingParams());
var g = Graphics.FromImage(image);
foreach (var xPosition in xPositions)
{
g.DrawLine(Pens.Red, (int)xPosition, 0, (int)xPosition, image.Height - 1);
}
g.Dispose();
image.Save(ExecutionDirInfoHelper.GetOutputDirPath() + outputFile);
}
public void TestMarkerListForStrippedMarioImage()
{
var backgroundColor = TestHelper.GetBackgroundColor();
var image = TestHelper.GetStrippedFirstMarioImage(backgroundColor);
const string outputFile = @"\TestMarkerListForStrippedMarioImage.png";
const int targetYVal = 20;
var variations = ColorVariationCalculator.GetColorVariationList(image, 0, image.Width - 1, image.Height - targetYVal);
var xPositions = MarkerProcessor.GetAllMarkerPositions(variations, new MarkerProcessingParams(), 0);
var g = Graphics.FromImage(image);
foreach (var xPosition in xPositions)
{
g.DrawLine(Pens.Red, (int)xPosition, 0, (int)xPosition, image.Height - 1);
}
g.Dispose();
image.Save(ExecutionDirInfoHelper.GetOutputDirPath() + outputFile);
}
public void Test()
{
//AR用カメラパラメタファイルをロード
NyARParam ap = NyARParam.createFromARParamFile(new StreamReader(camera_file));
ap.changeScreenSize(320, 240);
//試験イメージの読み出し(320x240 RGBのRAWデータ)
StreamReader sr = new StreamReader(data_file);
BinaryReader bs = new BinaryReader(sr.BaseStream);
byte[] raw = bs.ReadBytes(320 * 240 * 3);
NyARRgbRaster ra = new NyARRgbRaster(320, 240,NyARBufferType.BYTE1D_R8G8B8_24,false);
ra.wrapBuffer(raw);
MarkerProcessor pr = new MarkerProcessor(ap, ra.getBufferType());
pr.detectMarker(ra);
Console.WriteLine(pr.transmat.m00 + "," + pr.transmat.m01 + "," + pr.transmat.m02 + "," + pr.transmat.m03);
Console.WriteLine(pr.transmat.m10 + "," + pr.transmat.m11 + "," + pr.transmat.m12 + "," + pr.transmat.m13);
Console.WriteLine(pr.transmat.m20 + "," + pr.transmat.m21 + "," + pr.transmat.m22 + "," + pr.transmat.m23);
Console.WriteLine(pr.transmat.m30 + "," + pr.transmat.m31 + "," + pr.transmat.m32 + "," + pr.transmat.m33);
Console.WriteLine(pr.current_id);
return;
}
/// <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.");
}
