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); }