public static Matrix<double> KD_tree(Matrix<double> M_points, Matrix<double> P_points, bool _3D)
        {
            int Np = P_points.ColumnCount;
            int dimensions = _3D ? 3 : 2;
            //create tree of M_points
            var treeData2 = M_points.ToColumnArrays();
            var treeNodes = treeData2.Select(p => p.ToString()).ToArray();
            var m = Matrix<double>.Build;
            Matrix<double> Y = m.Dense(3, Np, 0);
            var tree = new KDTree<double, string>(dimensions, treeData2, treeNodes, L2Norm);
            var scan_data = P_points.ToColumnArrays();
            //Calculate nearest neighbour for every element of cloud
            for (int i = 0; i < scan_data.Length; i++)
            {
                var num = scan_data[i];
                var test = tree.NearestNeighbors(scan_data[i], 1);
                Y[0, i] = test[0].Item1[0];
                Y[1, i] = test[0].Item1[1];
            }
            return Y;//return result matrix

            // this function is used for calculating the Euclidean norm
            double L2Norm(double[] x, double[] y)
            {
                //square root is not used to speed up
                return x.Select((t, i) => (t - y[i]) * (t - y[i])).Sum();
            }
        }