PointSetFitterTest.cs

public class PointSetFitterTest
{
    [Test]
    public void MapPointSets()
    {
        var xs = new List<Point3D>
                 {
                     new Point3D(0, 0, 0),
                     new Point3D(1, 0, 0),
                     new Point3D(0, 1, 0),
                     new Point3D(0, 0, 1),
                     new Point3D(0, 0, 10)
                 };
        var cs = CoordinateSystem.Pitch(new AngleUnitValue(90, AngleUnit.Degrees));
        var ys = xs.Select(p => p.TransformBy(cs)).ToArray();
        var actual = FindTransform(xs, ys);
        LinearAlgebraAssert.AreEqual(cs, actual);
    }
    public static CoordinateSystem FindTransform(IList<Point3D> xs, IList<Point3D> ys)
    {
        var x = ToMatrix(xs);
        var y = ToMatrix(ys);
        var m = y.QR().Solve(x);
        return new CoordinateSystem(m);
    }
    private static DenseMatrix ToMatrix(IList<Point3D> points)
    {
        var m = new DenseMatrix(points.Count, 4);
        for (int i = 0; i < points.Count; i++)
        {
            var point = points[i];
            for (int j = 0; j < point.Count; j++)
            {
                m[i, j] = point[j];
            }
            m[i, 3] = 1;
        }
        return m;
    }
}