public void PointCloudFitting()
{
// https://github.com/ClayFlannigan/icp/blob/master/icp.py
// create original point cloud
var ppp0 = new Vector3[16];
var rnd = new Random(117);
for (int i = 0; i < ppp0.Length; ++i)
{
var x = (float)rnd.NextDouble() * 16 - 8;
var y = (float)rnd.NextDouble() * 16 - 8;
var z = (float)rnd.NextDouble() * 16 - 8;
ppp0[i] = new Vector3(x, y, z);
}
// create rotated point clound
var rot = Matrix4x4.CreateFromYawPitchRoll(1, 2, 3);
Matrix4x4.Invert(rot, out Matrix4x4 irot);
var ppp1 = new Vector3[ppp0.Length];
for (int i = 0; i < ppp0.Length; ++i)
{
ppp1[i] = Vector3.Transform(ppp0[i], rot);
}
// original point cloud tensor
var t0 = SpanTensor.Wrap(ppp0);
var t0t = new SpanTensor2 <float>(t0.Dimensions[1], t0.Dimensions[0]);
SpanTensor.Transpose(t0, t0t);
// rotated point cloud tensor
var t1 = SpanTensor.Wrap(ppp1);
var t1t = new SpanTensor2 <float>(t1.Dimensions[1], t1.Dimensions[0]);
SpanTensor.Transpose(t1, t1t);
// multiplied
var combined = new SpanTensor2 <float>(3, 3);
SpanTensor.MatrixMultiply(t1t, t0, combined);
// decomposition
var v = new SpanTensor2 <float>(3, 3);
var w = new float[3];
combined.SVD(w, v);
}
public void MatrixMultiplyTest()
{
var m1 = Matrix4x4.CreateFromYawPitchRoll(1, 2, 3) * Matrix4x4.CreateTranslation(2, 5, 6);
var m2 = Matrix4x4.CreateScale(1, 2, 1) * Matrix4x4.CreateFromYawPitchRoll(2, -1, 2);
var mr = m1 * m2;
var t1 = SpanTensor.Wrap(m1)[0];
var t2 = SpanTensor.Wrap(m2)[0];
var tr = SpanTensor.Wrap(Matrix4x4.Identity)[0];
SpanTensor.MatrixMultiply(t1, t2, tr);
var trm = SpanTensor.ToMatrix4x4(tr);
Assert.AreEqual(mr, trm);
}
public static void ApplySoftMax(this SpanTensor2 <float> tensor)
{
SpanTensor.ApplySoftMax(tensor.Span);
}
