MedianC#

using System;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;

namespace HelloWorld
{
    class Hello
    {
        public static void Main()
        {

            Console.WriteLine("Hi");

            Matrix<double> B = DenseMatrix.OfArray(new double[4, 3]{
        {-1.37285008021333,-2.39065444083597,-0.272229060927326},
        {-1.49265215830869,1.48381481188292,0.689109865473517},
        {2.69081228630851,-2.14018577957389,-0.326988358173307},
        {2.41142646810131,1.78497640136291,0.805298962324319},});

            Matrix<double> A = DenseMatrix.OfArray(new double[4, 3] {
        {-1.93017948067421,-2.05741846282552,0.460712729349417},
        {-2.11572517274675,2.01370167038341,0.420172921759525},
        {2.08424703950644,-2.15648187564387,-0.485352019388814},
        {1.95017255998246,2.22306603146633,-0.272585072006361}});

            int N = B.RowCount; //Total number of points

            Vector<double> X = A.ColumnSums() / N;      // Mean(A)
            Vector<double> Y = B.ColumnSums() / N;      // Mean(B)

            Matrix<double> centroid_A = DenseMatrix.OfArray(new double[N, 3]);

            for (int i = 0; i < N; i++)
            {
                centroid_A = centroid_A.InsertRow(i, X);
            }
            centroid_A = centroid_A.SubMatrix(0, N, 0, 3);  //centroid_A Nx3 of mean A values


            Matrix<double> centroid_B = DenseMatrix.OfArray(new double[N, 3]);

            for (int i = 0; i < N; i++)
            {
                centroid_B = centroid_B.InsertRow(i, Y);
            }

            centroid_B = centroid_B.SubMatrix(0, N, 0, 3);  //centroid_B Nx3 of mean B values

            Matrix<double> x1 = DenseMatrix.OfArray(new double[N, 3]);

            x1 = A - centroid_A;

            Matrix<double> x2 = DenseMatrix.OfArray(new double[N, 3]);

            x2 = B - centroid_B;

            Matrix<double> H;
            H = x1.Transpose() * x2; // Computing H

            var svd = H.Svd(); // Singular Value Decomposition

            var S = svd.S;  
            var U = svd.U;
            var V = svd.VT;

            Matrix<double> R;
            R = V.Transpose() * U.Transpose(); //Rotation Matrix

            Matrix<double> t;
            t = -1 * R * centroid_A.Transpose() + centroid_B.Transpose();

            Vector<double> t1;

            t1 = t.Column(0);  // Translation Vector

            Matrix<double> final_matrix = DenseMatrix.OfArray(new double[4, 4]);

            final_matrix = R;
            final_matrix = final_matrix.InsertColumn(3, t1); 

            double[] end = new double[4] { 0, 0, 0, 1 };
            Vector<double> end1 = DenseVector.OfArray(end);

            final_matrix = final_matrix.InsertRow(3, end1); // 4x4 Transformation matrix

            Console.WriteLine(final_matrix.ToString()); // Print the Matrix
    
            Console.WriteLine("Press enter to close...");
            Console.ReadLine();


        }
    }
}