public void ICPStep()
        {
            double[] s = { 1, 1, 1, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 };                 //scale
            double[] p = { 0, 0, 0, 0, 0, 0, 100, 100, 100 };                           //parameters

            //jam the values into vectors so that we can multiply them and shiz
            DenseVector scale = new DenseVector(s);
            DenseVector parameters = new DenseVector(p);

            //distance error in final iteration
            double fval_old = double.MaxValue;

            //change in distance error between two iterations
            double fval_percep = 0;

            //todo: some array to contain the transformed points

            //number of iterations
            int itt = 0;

            //get the max and min points of the static points
            double maxP = this.maxP();
            double minP = this.minP();

            double tolX = (maxP - minP) / 1000;
        }

        /// <summary>
        /// Run example
        /// </summary>
        public void Run()
        {
            // 1. Initialize a new instance of the empty vector with a given size
            var vector1 = new DenseVector(5);

            // 2. Initialize a new instance of the vector with a given size and each element set to the given value
            var vector2 = new DenseVector(5, 3.0);

            // 3. Initialize a new instance of the vector from an array.
            var vector3 = new DenseVector(new[] { 1.0, 2.0, 3.0, 4.0, 5.0 });

            // 4. Initialize a new instance of the vector by copying the values from another.
            var vector4 = new DenseVector(vector3);

            // Format vector output to console
            var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            Console.WriteLine(@"Vector 1");
            Console.WriteLine(vector1.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            Console.WriteLine(@"Vector 2");
            Console.WriteLine(vector2.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            Console.WriteLine(@"Vector 3");
            Console.WriteLine(vector3.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            Console.WriteLine(@"Vector 4");
            Console.WriteLine(vector4.ToString("#0.00\t", formatProvider));
            Console.WriteLine();
        }

        /// <summary>
        /// Передаются ссылки, point и delta будут изменены 
        /// </summary>
        /// <param name="delta"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        private Vector<double> ExplanotorySearch( Vector<double> point, Vector<double> delta)
        {
            var nvars = delta.Count;
            var prevbest = Fh.Y(point);
            var z = new DenseVector(nvars);

            point.CopyTo(z);

            int i;
            var minf = prevbest;

            for (i = 0; i < nvars; i++)
            {
                z[i] = point[i] + delta[i];
                var ftmp = Fh.Y(z);
                if (ftmp < minf)
                    minf = ftmp;
                else
                {
                    z[i] = point[i]  -delta[i];
                    ftmp = Fh.Y(z);
                    if (ftmp < minf)
                        minf = ftmp;
                    else
                    {
                        z[i] = point[i];
                    }
                }
            }

            return z;
        }

        private static void SetRealVertices(Workspace workspace)
        {
            Vector<double> fittedPlaneVector = GeometryHelper.FitPlaneToPoints(workspace.PointCloud.ToArray());

            if (fittedPlaneVector == null)
            {
                return;
            }

            Point3D projectedPoint = GeometryHelper.ProjectPoint3DToPlane(workspace.PointCloud.First(), fittedPlaneVector);

            Vector<double> planeNormal = new DenseVector(new[] { fittedPlaneVector[0], fittedPlaneVector[1], fittedPlaneVector[2] });

            Point3D[] vertices3D = workspace.Vertices3D;

            Point[] vertices = workspace.Vertices.ToArray();

            for (int i = 0; i < vertices.Length; i++)
            {

                Vector<double> pointOnPlane = new DenseVector(new[] { projectedPoint.X, projectedPoint.Y, projectedPoint.Z });
                Vector<double> pointOnLine = new DenseVector(new double[] { vertices3D[i].X, vertices3D[i].Y, vertices3D[i].Z });

                double d = (pointOnPlane.Subtract(pointOnLine)).DotProduct(planeNormal) / (pointOnLine.DotProduct(planeNormal));

                Vector<double> intersection = pointOnLine + pointOnLine.Multiply(d);

                workspace.FittedVertices[i] = new Point3D(intersection[0], intersection[1], intersection[2]);
            }

            workspace.PlaneVector = fittedPlaneVector;
        }

 public static DenseVector Normalize0to1(this DenseVector data)
 {
     var d = new DenseVector(data);
     var result = new DenseVector(d.Count);
     d.CopyTo(result);
     return (DenseVector) (result - d.Min())/(d.Max() - d.Min());
 }

        public void HeapSortWithDecreasingDoubleArray()
        {
            var sortedIndices = new int[10];
            var values = new DenseVector(10);
            values[0] = 9;
            values[1] = 8;
            values[2] = 7;
            values[3] = 6;
            values[4] = 5;
            values[5] = 4;
            values[6] = 3;
            values[7] = 2;
            values[8] = 1;
            values[9] = 0;
            for (var i = 0; i < sortedIndices.Length; i++)
            {
                sortedIndices[i] = i;
            }

            ILUTPElementSorter.SortDoubleIndicesDecreasing(0, sortedIndices.Length - 1, sortedIndices, values);
            for (var i = 0; i < sortedIndices.Length; i++)
            {
                Assert.AreEqual(i, sortedIndices[i], "#01-" + i);
            }
        }

        /// <summary>
        /// Missing mapping to P objects
        /// KdTree could be refactored to use P object instead of Math.Net
        /// 
        /// O(k * log n) 
        /// </summary>
        /// <param name="s"></param>
        /// <param name="origin"></param>
        /// <param name="k"></param>
        /// <param name="conf"></param>        
        /// <returns></returns>
        public long UpdateKnn(IAlgorithm s, IP origin, KnnConfiguration conf)
        {
            if (conf == null) conf = new KnnConfiguration();
            if (conf.SameTypeOnly) throw new NotImplementedException();
            if (conf.MaxDistance.HasValue) throw new NotImplementedException();

            var sw = new Stopwatch();
            sw.Start();

            var vector = new DenseVector(new[] { origin.X, origin.Y });
            var nn = Tree.FindNearestNNeighbors(vector, conf.K).ToList();

            s.Knn.Clear();
            s.Knn.Origin = origin;
            s.Knn.K = conf.K;
            foreach (var i in nn)
            {
                var p = new P { X = i[0], Y = i[1] };
                var dist = origin.Distance(p.X,p.Y);
                s.Knn.NNs.Add(new PDist {Point = p, Distance = dist});
            }

            sw.Stop();
            return sw.ElapsedMilliseconds;
        }

        /// <summary>
        /// 计算模糊矩阵
        /// </summary>
        private void CalculateFuzzyMatrix()
        {
            MemberShipFun _memeberShipFun = new MemberShipFun();
            for (int i = AHPIndexHierarchyUtil.totalLevelCount - 2; i >= 0; i--)
            {
                List<AHPIndexHierarchy> iLevelAhpIndexs = _ahpIndexUtil.FindbyLevel(i);
                foreach (AHPIndexHierarchy _iLevelAhpIndex in iLevelAhpIndexs)
                {
                    List<string> childrenNames = _iLevelAhpIndex.ChildrenNames;
                    DenseMatrix _iLevelMatrix = new DenseMatrix(childrenNames.Count, MemberShipFun.HealthLevelCount);
                    DenseVector _childrenValue = new DenseVector(childrenNames.Count);
                    for (int j = 0; j < childrenNames.Count; j++)
                    {
                        string name = childrenNames[j];
                        AHPIndexHierarchy _ahpIndex = _ahpIndexUtil.FindbyName(name);
                        if (i == AHPIndexHierarchyUtil.totalLevelCount - 2)//是底层
                        {
                            _ahpIndex.FuzzyValue = _memeberShipFun.TrapezoiMebership(_ahpIndex.IndexValue);
                        }

                        _iLevelMatrix = (DenseMatrix)_iLevelMatrix.InsertRow(j, _ahpIndex.FuzzyValue);
                        _iLevelMatrix = (DenseMatrix)_iLevelMatrix.RemoveRow(j + 1);
                        //_ahpIndex.ChildrenFuzzyMatrix = (DenseMatrix)_iLevelMatrix;
                        _childrenValue[j] = _ahpIndex.IndexValue;
                    }
                    _iLevelAhpIndex.ChildrenFuzzyMatrix = _iLevelMatrix;
                    _iLevelAhpIndex.IndexValue = _iLevelAhpIndex.ChildrenWeightVector * _childrenValue;
                    _iLevelAhpIndex.FuzzyValue = FuzzyOperator.WeightedAverage(_iLevelAhpIndex.ChildrenWeightVector, _iLevelAhpIndex.ChildrenFuzzyMatrix);
                }
            }
        }

        DenseMatrix[] sigmas; //covariance of the 3D gaussians.

        #endregion Fields

        #region Constructors

        public CD_HMM(MarkovChain A, DenseVector pi, DenseVector[] mus, DenseMatrix[] sigmas)
        {
            this.A = A;
            this.pi = pi;
            this.mus = mus;
            this.sigmas = sigmas;
        }

        public Point_Obj get_gaze_pt(Point_Obj right_eye, Point_Obj left_eye, float alpha, float beta, float gamma)
        {
            // Compute Z coordinate of head/eyes in the WCS (World Coordinate System)
            float Z = (-f * EYE_DIST) / (left_eye.get_x() - right_eye.get_x());
            //Console.WriteLine("EST Z: {0}", Z);

            // Use computed Z coordinate to get X,Y world coordinates of the eyes
            float X_r = Z * (right_eye.get_x() - princ_pt.get_x()) / (-f);
            float Y_r = Z * (right_eye.get_y() - princ_pt.get_y()) / (-f);

            float X_l = Z * (left_eye.get_x() - princ_pt.get_x()) / (-f);
            float Y_l = Z * (left_eye.get_y() - princ_pt.get_y()) / (-f);

            // Compute direction vector (d) of eyes using rotation angles (alpha, beta, gamma)
            DenseVector k_hat = new DenseVector(3);
            k_hat[2] = 1;
            //DenseMatrix R = get_rotation_mat(alpha, beta, gamma);
            DenseMatrix R = get_rotation_mat(alpha, beta, gamma);
            DenseVector d = R * k_hat;

            // Get point of intersection using eye points and direction vector d
            DenseVector P_r = new DenseVector(new[]{Convert.ToDouble(X_r), Convert.ToDouble(Y_r), Convert.ToDouble(Z)});
            DenseVector P_l = new DenseVector(new[] { Convert.ToDouble(X_l), Convert.ToDouble(Y_l), Convert.ToDouble(Z)});

            DenseVector p_hat_r = P_r + d * (-P_r[2] / d[2]);
            DenseVector p_hat_l = P_l + d * (-P_l[2] / d[2]);
            DenseVector p_hat_avg = (p_hat_r + p_hat_l) / 2;

            return new Point_Obj(Convert.ToSingle(p_hat_avg[0]), Convert.ToSingle(p_hat_avg[1]));
        }

 public double[] transform(double[] displacement, bool isreverse)
 {
     DenseMatrix xform = (isreverse ? (DenseMatrix)transformation.Inverse() : transformation);
     double[] dispcopy = new double[6];
     displacement.CopyTo(dispcopy, 0);
     DenseMatrix2String m2s = new DenseMatrix2String();
     List2String l2s = new List2String();
     DenseVector dispV = new DenseVector(dispcopy);
     log.Debug("original disp: " + l2s.ToString(displacement));
     if (isreverse)
     {
         dispV = (DenseVector)xform.Multiply(dispV);
         log.Debug("transformed Disp: " + l2s.ToString(dispV.Values));
     }
     DenseVector newDisp = translate(dispV.Values, isreverse);
     log.Debug("newDisp: " + l2s.ToString(newDisp.Values));
     DenseMatrix rollM = roll.create(dispV.Values[3]);
     DenseMatrix pitchM = pitch.create(dispV.Values[4]);
     DenseMatrix yawM = yaw.create(dispV.Values[5]);
     DenseMatrix rotation = (DenseMatrix)rollM.Multiply(pitchM.Multiply(yawM));
     log.Debug("rotation: " + m2s.ToString(rotation));
     DenseVector unt = (DenseVector)rotation.Multiply(directionalVector);
     log.Debug("unt: " + l2s.ToString(unt.Values));
     DenseVector newDisp1 = (DenseVector)unt.Add(newDisp);
     log.Debug("newDisp1: " + l2s.ToString(newDisp1.Values));
     dispV.SetSubVector(0, 3, newDisp1);
     if (isreverse == false)
     {
         dispV = (DenseVector)xform.Multiply(dispV);
     }
     log.Debug("resulting Disp: " + l2s.ToString(dispV.Values));
     return dispV.Values;
 }

        public List<Vector<double>> AddNoise(List<Vector<double>> points,
            double variance, int seed = 0)
        {
            List<Vector<double>> noisedPoints = new List<Vector<double>>(points.Count);
            int pointSize = points[0].Count;

            GaussianNoiseGenerator noise = new GaussianNoiseGenerator();
            noise.Variance = variance;
            noise.Mean = 0.0;
            noise.RandomSeed = seed != 0;
            noise.Seed = seed;
            noise.UpdateDistribution();

            for(int i = 0; i < _pointsCount; ++i)
            {
                Vector<double> cpoint = new DenseVector(pointSize);
                for(int p = 0; p < pointSize - 1; ++p)
                {
                    cpoint[p] = points[i][p] + noise.GetSample();
                }
                cpoint[pointSize - 1] = 1.0f;

                noisedPoints.Add(cpoint);
            }

            return noisedPoints;
        }

        /// <summary>
        /// Creates a new Input from double values.
        /// Note that we store each example as a row in the X matrix. While calculating Theta vector, we need to insert the top column of all ones into the X matrix - this will allow us to treat theta0 as just another feature.
        /// </summary>
        internal Input(double[,] x, double[] y, int skip, int take)
        {
            if (take == 0) {
                X = null;
                Y = null;
                return;
            }

            var samples = x.GetLength(0);
            var features = x.GetLength(1);

            //make sure we add first column of ones
            var x1 = new double[take, features + 1];
            var y1 = new double[take];

            for (int sample = 0; sample < samples; sample++) {
                if (sample < skip) {
                    continue;
                }
                for (int feature = 0; feature < features + 1; feature++) {
                    x1[sample - skip, feature] = (feature == 0) ? 1 : x[sample, feature - 1];
                }
                y1[sample - skip] = y[sample];

                take--;
                if (take == 0) {
                    break;
                }
            }

            X = new DenseMatrix(x1);
            Y = new DenseVector(y1).ToColumnMatrix();
        }

        public static void Run()
        {
            List<Position> list = new List<Position>();

            /*
            list.Add(new Position());
            list.Add(new Position("bond"));
            list.Add(new Position("stock"));
             * */
            Portfolio p = new Portfolio();

            double[] returns = {0.000, 0.13, -0.13};
            DenseVector returns1 = new DenseVector(returns);

            double[] stdev = {0, 7.4, 7.4};
            double[,] covariance = {{1, -.4, -.45}, {-.4, 1, .35}, {-0.45, 0.35, 1}};

            DenseMatrix covariance1 = StatisticsExtension.CorrelationToCovariance(new DenseMatrix(covariance),
                                                                                  new DenseVector(stdev));


            PortfolioOptimizer po = new PortfolioOptimizer(p, .09002, covariance1, returns1);
            po.BuildRiskModel();
            Console.ReadLine();
        }

        public void Optimize(Dictionary<double, double> values)
        {
            var n = _f.Functions.Count();
              var xs = values.Select(v => v.Key).ToList();
              var ys = values.Select(v => v.Value).ToList();
              var fs = new List<List<double>>(n);

              for (var i = 0; i < n; i++)
              {
            fs[i] = _f.Functions[i].Evaluate(xs);
              }

              var matrix = new DenseMatrix(n, n);
              var vector = new DenseVector(n);
              for (var i = 0; i < n; i++)
              {
            for (var j = 0; j < n; j++)
            {
              matrix[i, j] = fs[i].ScalarProduct(fs[j]);
            }
            vector[i] = ys.ScalarProduct(fs[i]);
              }

              var matrixInverse = matrix.Inverse();

              var result = matrixInverse * vector;

              for (var i = 0; i < n; i++)
              {
            _f.LinearParameters[i].Value = result[i];
              }
        }

        public static Vector<double> refsignal(Timer0Freq Timer0, Timer1Freq Timer1, Timer3Freq Timer3, string code, double Fs)
        {
            double f0 = Convert.ToDouble(Timer0);
            double f1 = Convert.ToDouble(Timer1);
            double f3 = Convert.ToDouble(Timer3);
            double T = 1 / Fs;
            int samplesperbit = Convert.ToInt32(Math.Round(Fs / f1));
            int samples = (int)Math.Ceiling(Fs / f3);
            Vector<double> signal = new DenseVector(samples);

            string bincodestring = Convert.ToString(Convert.ToInt64(code, 16), 2);// Convert.ToString(code, 2); //string.Join("",code.Reverse().Select(x => Convert.ToString(x, 2).PadLeft(8, '0')));
            int bitnumber = 0;
            for (int ind = 0; ind < bincodestring.Length; ind++)
            {
                char s = bincodestring[ind];
                bitnumber++;
                if (s == '1')
                {
                    int indexfrom = (bitnumber - 1) * samplesperbit + 1;
                    int indexto = bitnumber * samplesperbit;
                    for (int sample = indexfrom; sample <= indexto; sample++)
                    {
                        signal[sample] = 1.0f;
                    }
                }
            }
            for (int i = 0; i < samples; i++)
            {
                double carrier = Math.Round(Math.Cos(2 * Math.PI * f0 / Fs * i) / 2 + 0.5);
                signal[i] *= carrier;
            }
            return signal;
        }

 private static void GetLinRegArgs(IEnumerable<double[]> items, out DenseMatrix xM, out DenseVector yV)
 {
     var original = items;
       var transformed = original.Select(a => new Tuple<double[], double>(Phi(a), a[2])).ToList();
       xM = DenseMatrix.Create(transformed.Count, transformed[0].Item1.Length, (r, c) => transformed[r].Item1[c]);
       yV = DenseVector.OfEnumerable(transformed.Select(t => t.Item2));
 }

        public void SolveWideMatrixThrowsArgumentException()
        {
            var matrix = new SparseMatrix(2, 3);
            var input = new DenseVector(2);

            var solver = new TFQMR();
            Assert.That(() => matrix.SolveIterative(input, solver), Throws.ArgumentException);
        }

        public void SolveLongMatrixThrowsArgumentException()
        {
            var matrix = new SparseMatrix(3, 2);
            var input = new DenseVector(3);

            var solver = new GpBiCg();
            Assert.Throws<ArgumentException>(() => matrix.SolveIterative(input, solver));
        }

 public RigidTransform(double[] motionCenter, double[,] transformation)
 {
     this.motionCenter = new DenseVector(motionCenter);
     this.transformation = DenseMatrix.OfArray(transformation);
     List2String l2s = new List2String();
     log.Debug("motionCenter: " + l2s.ToString(this.motionCenter.Values) +
         " platformCenter: " + l2s.ToString(this.platformCenter.Values));
 }