public void Test_RigidTransform()
{
var p1 = new PosXYZ(1, 1, 0);
var p2 = new PosXYZ(1, 5, 0);
var p3 = new PosXYZ(1, 10, 0);
Console.WriteLine($"p1:{p1}");
Console.WriteLine($"p2:{p2}");
Console.WriteLine($"p3:{p3}");
var p1r = p1.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
var p2r = p2.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
var p3r = p3.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
Console.WriteLine($"p1r:{p1r}");
Console.WriteLine($"p2r:{p2r}");
Console.WriteLine($"p3r:{p3r}");
var ret = XyzPlarformCalibration.CalcAlignTransform(new List <PosXYZ>()
{
p1, p2, p3
}, new List <PosXYZ>()
{
p1r, p2r, p3r
});
for (int i = 0; i < ret.Item1.GetLength(0); i++)
{
for (int j = 0; j < ret.Item1.GetLength(1); j++)
{
Console.Write(ret.Item1[i, j].ToString("F3"));
Console.Write(",");
}
Console.WriteLine();
}
Console.WriteLine();
}
public void Test_AffineTransformWithRot()
{
List <PosXYZ> cpuPos = new List <PosXYZ>()
{
new PosXYZ(62.69, -64.8, 0.987),
new PosXYZ(62.69, -96.8, 1.060),
new PosXYZ(94.69, -96.8, 1.108),
new PosXYZ(94.69, -64.8, 1.009),
};
for (int i = 0; i < cpuPos.Count; i++)
{
cpuPos[i] = cpuPos[i].Scale(new PosXYZ(10.3, 10.31, 1)).RotateAt(new PosXYZ(), 42.2).Translate(new PosXYZ(10, 11.2d, 0));
}
List <PosXYZ> oldcpupos = new List <PosXYZ>()
{
new PosXYZ(62.69, -64.8, 0.987),
new PosXYZ(62.69, -96.8, 1.060),
new PosXYZ(94.69, -96.8, 1.108),
new PosXYZ(94.69, -64.8, 1.009),
};
{
var ret = XyzPlarformCalibration.CalcAffineTransform(cpuPos, oldcpupos);
Console.WriteLine($"CalcAffineTransform:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n");
Console.WriteLine($"Error:{ret.Item2:F6}");
var mat = DenseMatrix.OfArray(new double[, ]
{
{ ret.Item1[0, 0], ret.Item1[0, 1], ret.Item1[0, 3] },
{ ret.Item1[1, 0], ret.Item1[1, 1], ret.Item1[1, 3] },
{ 0, 0, 1 }
}).Inverse();
var newMat = DenseMatrix.OfArray(new double[, ]
{
{ mat[0, 0], mat[0, 1], mat[0, 2] },
{ mat[1, 0], mat[1, 1], mat[1, 2] },
});
Console.WriteLine($"InverseMatrix:\r\n{newMat.ToString()}");
}
{
var ret = XyzPlarformCalibration.CalcAffineTransform(oldcpupos, cpuPos);
Console.WriteLine($"CalcAffineTransform Inverse:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n");
}
{
var ret = XyzPlarformCalibration.CalcAlignTransform(cpuPos, oldcpupos);
Console.WriteLine($"CalcAlignTransform:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6}\r\n");
Console.WriteLine($"Error:{ret.Item2:F6}");
}
}