C# DenseMatrix.SetColumn方法代码示例

        public void build_prob_map()
        {
            Normal N_x = new Normal(X / 2, STD_X);
            Normal N_y = new Normal(Y / 2, STD_Y);

            DenseMatrix M_x = new DenseMatrix(Y, X, 0.0);
            DenseMatrix M_y = new DenseMatrix(Y, X, 0.0);

            DenseVector V_x = new DenseVector(X);
            for (int i = 0; i < X; i++)
            {
                V_x[i] = N_x.Density(i);
            }

            for (int j = 0; j < Y; j++)
            {
                M_x.SetRow(j, V_x);
            }

            DenseVector V_y = new DenseVector(Y);
            for (int i = 0; i < Y; i++)
            {
                V_y[i] = N_y.Density(i);
            }

            for (int j = 0; j < X; j++)
            {
                M_y.SetColumn(j, V_y);
            }

            DenseMatrix MULT = (DenseMatrix)M_x.PointwiseMultiply(M_y);
            double s = MULT.Data.Sum();
            MULT = (DenseMatrix)MULT.PointwiseDivide(new DenseMatrix(Y, X, s));
            //this.dataGridView1.DataSource = MULT;
            //Console.WriteLine(MULT.Data.Sum());
            PROB_MAP_ORIG = MULT;

            s = MULT[Y / 2, X / 2];
            MULT = (DenseMatrix)MULT.PointwiseDivide(new DenseMatrix(Y, X, s));

            /*
            for (int i = 0; i < Y; i++)
            {
                Console.Write(i + " - ");
                for (int j = 0; j < X; j++)
                {
                    Console.Write(MULT[i, j] + " ");
                }
                Console.WriteLine();
                Console.WriteLine();
            }
            */
            PROB_MAP = MULT;
        }

		public void Smooth(ref double[,] inputValues)
		{
			// TODO: Using the matrix works, but does a lot of data accesses. Can improve by working out all the data access myself? I might be able to cut down on number of data accesses, but not sure.
		    var inputMatrix = new DenseMatrix(inputValues.GetLength(0), inputValues.GetLength(1));

			for (int i = 0; i < inputMatrix.RowCount; i++)
			{
				inputMatrix.SetRow(i, Smooth(inputMatrix.Row(i).ToArray()));
			}

			for (int i = 0; i < inputMatrix.ColumnCount; i++)
			{
				inputMatrix.SetColumn(i, Smooth(inputMatrix.Column(i).ToArray()));
			}

			inputValues = inputMatrix.ToArray();
		}

//.........这里部分代码省略.........
            int newEntering, exitingRow;

            bool optimal = false;

            if(artifical)
            {
                rhsOverPPrime = new double[numConstraints + 1];
            }
            else
            {
                rhsOverPPrime = new double[numConstraints];
            }

            while (!optimal)
            {
                //calculates the inverse of b for this iteration
                bInverse = (DenseMatrix)b.Inverse();

                //updates the C vector with the most recent basic variables
                cCounter = 0;
                foreach (int index in basics)
                {
                    cBVect[cCounter++] = objFunValues.At(index);
                }

                //calculates the pPrimes and cPrimes
                for (int i = 0; i < coefficients.ColumnCount; i++)
                {
                    if (!basics.Contains(i))
                    {
                        pPrimes[i] = (DenseMatrix)bInverse.Multiply((DenseMatrix)coefficients.Column(i).ToColumnMatrix());

                        //c' = objFunVals - cB * P'n
                        //At(0) to turn it into a double
                        cPrimes[i] = objFunValues.At(i) - (pPrimes[i].LeftMultiply(cBVect)).At(0);
                    }
                    else
                    {
                        pPrimes[i] = null;
                    }
                }

                //RHS'
                xPrime = (DenseMatrix)bInverse.Multiply((DenseMatrix)rhsValues.ToColumnMatrix());

                //Starts newEntering as the first nonbasic
                newEntering = -1;
                int iter = 0;
                while(newEntering == -1)
                {
                    if(!basics.Contains(iter))
                    {
                        newEntering = iter;
                    }

                    iter++;
                }

                //new entering becomes the small cPrime that corresponds to a non-basic value
                for (int i = 0; i < cPrimes.Length; i++)
                {
                    if (cPrimes[i] < cPrimes[newEntering] && !basics.Contains(i))
                    {
                        newEntering = i;
                    }
                }

                //if the smallest cPrime is >= 0, ie they are all positive
                if (cPrimes[newEntering] >= 0)
                {
                    optimal = true;
                }
                else
                {
                    //fix me to deal with if all these values are negative
                    exitingRow = 0;
                    for (int i = 0; i < xPrime.RowCount; i++)
                    {
                        double[,] pPrime = pPrimes[newEntering].ToArray();
                        rhsOverPPrime[i] = xPrime.ToArray()[i, 0] / pPrime[i, 0];

                        if (rhsOverPPrime[i] < rhsOverPPrime[exitingRow] && rhsOverPPrime[i] > 0 )
                        {
                            exitingRow = i;
                        }
                    }

                    //translates from the index in the basics list to the actual row
                    exitingRow = basics[exitingRow];

                    //make sure you're not being stupid here!!!!
                    int tempIndex = basics.IndexOf(exitingRow);
                    basics.Remove(exitingRow);

                    basics.Insert(tempIndex, newEntering);

                    b.SetColumn(basics.IndexOf(newEntering), coefficients.Column(newEntering));
                }
            }
        }

        private Matrix<double> CameraMatrixFromParameterization(int n0)
        {
            // Parameters:
            // Full: [fx, fy, s, px, py, eaX, eaY, eaZ, Cx, Cy, Cz]
            // Center fixed: [fx, fy, s, eaX, eaY, eaZ, Cx, Cy, Cz]
            // Aspect fixed: [f, eaX, eaY, eaZ, Cx, Cy, Cz]
            //
            // Camera : M = KR[I|-C]
            double fx = ParametersVector.At(n0 + _fxIdx);
            double fy = ParametersVector.At(n0 + _fyIdx);
            double sk = ParametersVector.At(n0 + _skIdx);
            double px = ParametersVector.At(n0 + _pxIdx);
            double py = ParametersVector.At(n0 + _pyIdx);
            double eX = ParametersVector.At(n0 + _euXIdx);
            double eY = ParametersVector.At(n0 + _euYIdx);
            double eZ = ParametersVector.At(n0 + _euZIdx);
            Vector<double> euler = new DenseVector(new double[3] { eX, eY, eZ });
            double cX = ParametersVector.At(n0 + _cXIdx);
            double cY = ParametersVector.At(n0 + _cYIdx);
            double cZ = ParametersVector.At(n0 + _cZIdx);
            Vector<double> center = new DenseVector(new double[3] { -cX, -cY, -cZ });

            Matrix<double> intMat = new DenseMatrix(3, 3);
            intMat.At(0, 0, fx);
            intMat.At(0, 1, sk);
            intMat.At(0, 2, px);
            intMat.At(1, 1, fy);
            intMat.At(1, 2, py);
            intMat.At(2, 2, 1.0);

            Matrix<double> rotMat = new DenseMatrix(3, 3);
            RotationConverter.EulerToMatrix(euler, rotMat);

            Matrix<double> extMat = new DenseMatrix(3, 4);
            extMat.SetSubMatrix(0, 0, rotMat);
            extMat.SetColumn(3, rotMat * center);

            return intMat * extMat;
        }

        /// <summary>
        /// Run example
        /// </summary>
        public void Run()
        {
            // Format matrix output to console
            var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";
            
            // Create square matrix
            var matrix = new DenseMatrix(5);
            var k = 0;
            for (var i = 0; i < matrix.RowCount; i++)
            {
                for (var j = 0; j < matrix.ColumnCount; j++)
                {
                    matrix[i, j] = k++;
                }
            }

            Console.WriteLine(@"Initial matrix");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Create vector
            var vector = new DenseVector(new[] { 50.0, 51.0, 52.0, 53.0, 54.0 });
            Console.WriteLine(@"Sample vector");
            Console.WriteLine(vector.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 1. Insert new column
            var result = matrix.InsertColumn(3, vector);
            Console.WriteLine(@"1. Insert new column");
            Console.WriteLine(result.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Insert new row
            result = matrix.InsertRow(3, vector);
            Console.WriteLine(@"2. Insert new row");
            Console.WriteLine(result.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 3. Set column values
            matrix.SetColumn(2, (Vector)vector);
            Console.WriteLine(@"3. Set column values");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 4. Set row values. 
            matrix.SetRow(3, (double[])vector);
            Console.WriteLine(@"4. Set row values");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 5. Set diagonal values. SetRow/SetColumn/SetDiagonal accepts Vector and double[] as input parameter
            matrix.SetDiagonal(new[] { 5.0, 4.0, 3.0, 2.0, 1.0 });
            Console.WriteLine(@"5. Set diagonal values");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 6. Set submatrix values
            matrix.SetSubMatrix(1, 3, 1, 3, DenseMatrix.Identity(3));
            Console.WriteLine(@"6. Set submatrix values");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Permutations. 
            // Initialize a new instance of the Permutation class. An array represents where each integer is permuted too: 
            // indices[i] represents that integer "i" is permuted to location indices[i]
            var permutations = new Permutation(new[] { 0, 1, 3, 2, 4 });
            
            // 7. Permute rows 3 and 4
            matrix.PermuteRows(permutations);
            Console.WriteLine(@"7. Permute rows 3 and 4");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 8. Permute columns 1 and 2, 3 and 5
            permutations = new Permutation(new[] { 1, 0, 4, 3, 2 });
            matrix.PermuteColumns(permutations);
            Console.WriteLine(@"8. Permute columns 1 and 2, 3 and 5");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 9. Concatenate the matrix with the given matrix
            var append = matrix.Append(matrix);

            // Concatenate into result matrix
            matrix.Append(matrix, append);
            Console.WriteLine(@"9. Append matrix to matrix");
            Console.WriteLine(append.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

             // 10. Stack the matrix on top of the given matrix matrix
            var stack = matrix.Stack(matrix);

            // Stack into result matrix
            matrix.Stack(matrix, stack);
            Console.WriteLine(@"10. Stack the matrix on top of the given matrix matrix");
            Console.WriteLine(stack.ToString("#0.00\t", formatProvider));
//.........这里部分代码省略.........

 /// <summary>
 /// adds new column to matrix
 /// </summary>
 /// <param name="dest">matrix which add column</param>
 /// <param name="colToAdd">column added to matrix</param>
 /// <returns>new matrix</returns>
 private static Matrix AddColumn(Matrix dest, Vector colToAdd)
 {
     Matrix res = new DenseMatrix(dest.RowCount, dest.ColumnCount + 1);
     res.SetSubMatrix(0, dest.RowCount, 0, dest.ColumnCount, dest);
     res.SetColumn(res.ColumnCount - 1, colToAdd);
     return res;
 }

        /// <summary>
        /// Adaptive Cross Approximation (ACA) matrix compression
        /// the result is stored in U and V matrices like U*V
        /// </summary>
        /// <param name="acaThres">Relative error threshold to stop adding rows and columns in ACA iteration</param>
        /// <param name="m">Row indices of Z submatrix to compress</param>
        /// <param name="n">Column indices of Z submatrix to compress</param>
        /// <param name="U">to store result</param>
        /// <param name="V">to store result</param>
        /// <returns>pair with matrix U and V</returns>
        public static Tuple<Matrix, Matrix> Aca(double acaThres, List<int> m, List<int> n, Matrix U, Matrix V)
        {
            int M = m.Count;
            int N = n.Count;
            int Min = Math.Min(M, N);
            U = new DenseMatrix(Min, Min);
            V = new DenseMatrix(Min, Min);
            //if Z is a vector, there is nothing to compress
            if (M == 1 || N == 1)
            {
                U = UserImpedance(m, n);
                V = new DenseMatrix(1, 1);
                V[0, 0] = 1.0;
                return new Tuple<Matrix,Matrix>(U,V);
            }

            //Indices of columns picked up from Z
            //Vector J = new DenseVector(N);
            //List<int> J = new List<int>(N);

            List<int> J = new List<int>(new int [N]);
            //int[] J = new int[N];
            //Indices of rows picked up from Z
            //Vector I = new DenseVector(M);
            List<int> I = new List<int>(new int [M]);
            //int[] I = new int[M];
            //Row indices to search for maximum in R
            //Vector i = new DenseVector(M);
            List<int> i = new List<int>();
            //int[] i = new int[M];
            //Column indices to search for maximum in R
            //Vector j = new DenseVector(N);
            List<int> j = new List<int>();
            //int[] j = new int[N];

            for (int k = 1; k < M; k++)
            {
                i.Add(k);
            }

            for (int k = 0; k < N; k++)
            {
                j.Add(k);
            }

            //Initialization

            //Initialize the 1st row index I(1) = 1
            I[0] = 0;

            //Initialize the 1st row of the approximate error matrix
            List<int> m0 = new List<int>();
            m0.Add(m[I[0]]);
            Matrix Rik = UserImpedance(m0, n);

            //Find the 1st column index J(0)
            double max = -1.0;
            int col = 0;

            foreach (int ind in j)
            {
                if (Math.Abs(Rik[0, ind]) > max)
                {
                    max = Math.Abs(Rik[0, ind]);
                    col = ind;
                }
            }

            //J[0] = j[col];
            J[0] = col;
            j.Remove(J[0]);

            //First row of V
            V = new DenseMatrix(1, Rik.ColumnCount);
            V.SetRow(0, Rik.Row(0).Divide(Rik[0, J[0]]));

            //Initialize the 1st column of the approximate error matrix
            List<int> n0 = new List<int>();
            n0.Add(n[J[0]]);
            Matrix Rjk = UserImpedance(m, n0);

            //First column of U
            U = new DenseMatrix(Rjk.RowCount, 1);
            U.SetColumn(0, Rjk.Column(0));

            // Norm of (approximate) Z, to test error
            double d1 = U.L2Norm();
            double d2 = V.L2Norm();
            double normZ = d1 * d1 * d2 * d2;

//.........这里部分代码省略.........

        public void placeCompensatorPoles(double value)
        {
            // Check for dimensions
            if (A.ColumnCount != A.RowCount || A.ColumnCount <= 0) {
                // TODO: Throw exception
            }

            int n = A.ColumnCount;

            // Calculate controllability matrix
            Matrix<double> controllabilityMatrix = new DenseMatrix(n, n);
            for (int i = 0; i < n; i++) {
                Vector<double> vec = B.Column(0);

                for (int j = 0; j < i; j++) {
                    vec = A * vec;
                }

                controllabilityMatrix.SetColumn(i, vec);
            }

            // Unity vector
            Matrix<double> unityVector = new DenseMatrix(1,n);
            for (int i = 0; i < n; i++) {
                // Set 1 at last index
                unityVector.At(0, i,
                    (i == n-1 ? 1 : 0)
                    );
            }

            // Coefficients matrix
            Matrix<double> preparedMatrix = A.Clone();
            for (int i = 0; i < n; i++) {
                // Substract value from diagonal
                preparedMatrix.At(i, i,
                    preparedMatrix.At(i, i) - value
                    );
            }
            Matrix<double> coefficientsMatrix = preparedMatrix.Clone();
            for (int i = 0; i < n-1; i++) {
                // Multiply n-1 times
                coefficientsMatrix = preparedMatrix * coefficientsMatrix;
            }

            // Calculate new K using Ackermann's formula
            K = unityVector * controllabilityMatrix.Inverse() * coefficientsMatrix;
        }

        public void CreateCameraMatrices()
        {
            _K_L = new DenseMatrix(3, 3);
            _K_L[0, 0] = 10.0; // fx
            _K_L[1, 1] = 10.0; // fy
            _K_L[0, 1] = 0.0; // s
            _K_L[0, 2] = 300.0; // x0
            _K_L[1, 2] = 250.0; // y0
            _K_L[2, 2] = 1.0; // 1

            _K_R = new DenseMatrix(3, 3);
            _K_R[0, 0] = 10.0; // fx
            _K_R[1, 1] = 10.5; // fy
            _K_R[0, 1] = 0.0; // s
            _K_R[0, 2] = 300.0; // x0
            _K_R[1, 2] = 200.0; // y0
            _K_R[2, 2] = 1.0; // 1

            _R_L = DenseMatrix.CreateIdentity(3);
            _R_R = DenseMatrix.CreateIdentity(3);

            _C_L = new DenseVector(3);
            _C_L[0] = 50.0;
            _C_L[1] = 50.0;
            _C_L[2] = 0.0;

            _C_R = new DenseVector(3);
            _C_R[0] = 0.0;
            _C_R[1] = 40.0;
            _C_R[2] = 10.0;

            Matrix<double> Ext_l = new DenseMatrix(3, 4);
            Ext_l.SetSubMatrix(0, 0, _R_L);
            Ext_l.SetColumn(3, -_R_L * _C_L);

            Matrix<double> Ext_r = new DenseMatrix(3, 4);
            Ext_r.SetSubMatrix(0, 0, _R_R);
            Ext_r.SetColumn(3, -_R_R * _C_R);

            _CM_L = _K_L * Ext_l;
            _CM_R = _K_R * Ext_r;
        }

//.........这里部分代码省略.........
            //    }
            //}

            // Шаг 3
            var falseIndex = -1;
            var maxFract = 0d;
            for (int i = 0; i < _task.xo.Count(); i++)
            {
                if (Math.Abs(Math.Round(_task.xo[i]) - _task.xo[i]) > Eps)
                {
                    var fract = Math.Abs(_task.xo[i] - Math.Floor(_task.xo[i]));    // Находим базисную переменную
                    if (_task.Jb.Contains(i) && fract > Eps)                        // С максимальной дробной частью
                    {                                                               // и запоминаем ее индекс
                        if (fract > maxFract)
                        {
                            maxFract = fract;
                            falseIndex = i;
                        }
                    }
                }
            }

            if (falseIndex < 0)     // Если все переменные целые - решение найдено
            {
                return false;   // Прерываем выполнение метода
            }
            _writer.WriteLine("Jk = {0}", falseIndex);

            // Шаг 4
            var aB = new DenseMatrix(_task.Jb.Count());
            int index = 0;
            foreach (var j in _task.Jb)
            {
                aB.SetColumn(index, _task.A.Column(j));     // Формируем матрицу Ab из базисных столбцов А
                index++;
            }
            _writer.Write("Jb: ");
            _task.Jb.ForEach(x => _writer.Write("{0} ", x));
            _writer.WriteLine();
            _writer.WriteLine("Basis matrix: {0}", aB);
            var y = DenseMatrix.Identity(_task.A.RowCount).Column(_task.Jb.IndexOf(falseIndex)) * aB.Inverse(); //Находим e'*Ab

            var newRow = new DenseVector(_task.A.ColumnCount + 1);
            newRow.SetSubVector(0, _task.A.ColumnCount, y * _task.A);   // Находим данные для нового отсекающего ограничения

            _writer.WriteLine("Data for new limitation: {0}", newRow);

            for (int i = 0; i < newRow.Count; i++)      // Формируем новое отсекающее ограничение
            {
                if (i < _task.A.ColumnCount)
                {
                    if (Math.Abs(newRow[i]) < Eps)
                    {
                        newRow[i] = 0;
                    }
                    else
                    {
                        newRow[i] = newRow[i] > 0
                                    ? -(newRow[i] - Math.Floor(newRow[i]))
                                    : -(Math.Ceiling(Math.Abs(newRow[i])) - Math.Abs(newRow[i]));
                    }
                }
                else
                {
                    newRow[i] = 1;
                }

 private DenseMatrix GetBaseMatrix(DenseMatrix matrix, List<int> baseJ)
 {
     var resM = new DenseMatrix(baseJ.Count);
     int i = 0;
     foreach (var j in baseJ)
     {
         resM.SetColumn(i, matrix.Column(j));
         i++;
     }
     return resM;
 }

        public void Test_Rectification()
        {
            var Fi = new DenseMatrix(3); // Target F
            Fi[1, 2] = -1.0;
            Fi[2, 1] = 1.0;

            var K_l = new DenseMatrix(3, 3);
            K_l[0, 0] = 10.0; // fx
            K_l[1, 1] = 10.0; // fy
            K_l[0, 1] = 0.0; // s
            K_l[0, 2] = 300.0; // x0
            K_l[1, 2] = 250.0; // y0
            K_l[2, 2] = 1.0; // 1

            var K_r = new DenseMatrix(3, 3);
            K_r[0, 0] = 12.0; // fx
            K_r[1, 1] = 12.5; // fy
            K_r[0, 1] = 0.0; // s
            K_r[0, 2] = 300.0; // x0
            K_r[1, 2] = 200.0; // y0
            K_r[2, 2] = 1.0; // 1

            var R_l = DenseMatrix.CreateIdentity(3);
            var R_r = DenseMatrix.CreateIdentity(3);

            Vector<double> C_l = new DenseVector(3);
            C_l[0] = 50.0;
            C_l[1] = 50.0;
            C_l[2] = 0.0;

            Vector<double> C_r = new DenseVector(3);
            C_r[0] = 40.0;
            C_r[1] = 40.0;
            C_r[2] = 10.0;

            Matrix<double> Ext_l = new DenseMatrix(3, 4);
            Ext_l.SetSubMatrix(0, 0, R_l);
            Ext_l.SetColumn(3, -R_l * C_l);

            Matrix<double> Ext_r = new DenseMatrix(3, 4);
            Ext_r.SetSubMatrix(0, 0, R_r);
            Ext_r.SetColumn(3, -R_r * C_r);

            var CM_l = K_l * Ext_l;
            var CM_r = K_r * Ext_r;

            // Find e_R = P_R*C_L, e_L = P_L*C_R
            var epi_r = CM_r * new DenseVector(new double[] { C_l[0], C_l[1], C_l[2], 1.0 });
            var epi_l = CM_l * new DenseVector(new double[] { C_r[0], C_r[1], C_r[2], 1.0 });

            var ex_l = new DenseMatrix(3, 3);
            ex_l[0, 0] = 0.0;
            ex_l[1, 0] = epi_l[2];
            ex_l[2, 0] = -epi_l[1];
            ex_l[0, 1] = -epi_l[2];
            ex_l[1, 1] = 0.0;
            ex_l[2, 1] = epi_l[0];
            ex_l[0, 2] = epi_l[1];
            ex_l[1, 2] = -epi_l[0];
            ex_l[2, 2] = 0.0;

            var ex_r = new DenseMatrix(3, 3);
            ex_r[0, 0] = 0.0;
            ex_r[1, 0] = epi_r[2];
            ex_r[2, 0] = -epi_r[1];
            ex_r[0, 1] = -epi_r[2];
            ex_r[1, 1] = 0.0;
            ex_r[2, 1] = epi_r[0];
            ex_r[0, 2] = epi_r[1];
            ex_r[1, 2] = -epi_r[0];
            ex_r[2, 2] = 0.0;

            // F = [er]x * Pr * pseudoinv(Pl)
            var F = ex_r * (CM_r * CM_l.PseudoInverse());
            int rank = F.Rank();
            if(rank == 3)
            {
                // Need to ensure rank 2, so set smallest singular value to 0
                var svd = F.Svd();
                var E = svd.W;
                E[2, 2] = 0;
                var oldF = F;
                F = svd.U * E * svd.VT;
                var diff = F - oldF; // Difference should be very small if all is correct
            }

            // Scale F, so that F33 = 1
            F = F.Divide(F[2, 2]);

            ImageRectification_ZhangLoop rect = new ImageRectification_ZhangLoop();
            // Assume image of size 640x480
            rect.ImageHeight = 480;
            rect.ImageWidth = 640;
            rect.FundamentalMatrix = F;
            rect.EpiCrossLeft = ex_l;

            rect.ComputeRectificationMatrices();

            // Test H'^T * Fi * H should be very close to F
            var H_r = rect.RectificationRight;
//.........这里部分代码省略.........

        public void Test_EstimateCameraMatrix_NoisedLinear()
        {
            PrepareCameraMatrix();
            var points = GenerateCalibrationPoints_Random();
            PrepareCalibrator(
                AddNoise(points, _varianceReal, _varianceImage));

            _calib.HomoPoints();

            _calib.NormalizeImagePoints();
            _calib.NormalizeRealPoints();

            _calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();

            _calib.DenormaliseCameraMatrix();
            _calib.DecomposeCameraMatrix();
            _eCM = _calib.CameraMatrix;

            double scaleK = 1.0 / _calib.CameraInternalMatrix[2, 2];
            _eCM.MultiplyThis(-scaleK);

            var eK = _calib.CameraInternalMatrix.Multiply(scaleK);
            var eR = -_calib.CameraRotationMatrix;
            var eC = -(_eCM.SubMatrix(0, 3, 0, 3).Inverse() * _eCM.Column(3));

            Matrix<double> eExt = new DenseMatrix(3, 4);
            eExt.SetSubMatrix(0, 0, eR);
            eExt.SetColumn(3, -eR * eC);

            var eCM = eK * eExt;

            var errVec = _CM.PointwiseDivide_NoNaN(_eCM);
            double err = errVec.L2Norm();
            Assert.IsTrue(
                Math.Abs(err - Math.Sqrt(12)) < Math.Sqrt(12) / 50.0 || // max 2% diffrence
                (_eCM - _CM).FrobeniusNorm() < 1e-3);

            for(int p = 0; p < _pointsCount; ++p)
            {
                var cp = points[p];
                Vector<double> rp = new DenseVector(4);
                rp[0] = cp.RealX;
                rp[1] = cp.RealY;
                rp[2] = cp.RealZ;
                rp[3] = 1.0;
                var imagePoint = _eCM * rp;

                Vector2 ip = new Vector2(imagePoint[0] / imagePoint[2], imagePoint[1] / imagePoint[2]);
                Assert.IsTrue((ip - cp.Img).Length() < cp.Img.Length() / 10.0);
            }
        }

        public void Test_EstimateCameraMatrix_Minimised()
        {
            PrepareCameraMatrix();
            var points = GenerateCalibrationPoints_Random(100);
            var noisedPoints = AddNoise(points, _varianceReal, _varianceImage, 200);
            PrepareCalibrator(noisedPoints);

            _calib.HomoPoints();
            _calib.NormalizeImagePoints();
            _calib.NormalizeRealPoints();
            _calib._pointsNormalised = true;
            _calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();
            //    _calib.FindNormalisedVariances();
            _calib.DenormaliseCameraMatrix();
            _calib._pointsNormalised = false;

            _eCM = _calib.CameraMatrix;
            double totalDiff = 0.0;
            for(int p = 0; p < _pointsCount; ++p)
            {
                var cp = points[p];
                Vector<double> rp = new DenseVector(4);
                rp[0] = cp.RealX;
                rp[1] = cp.RealY;
                rp[2] = cp.RealZ;
                rp[3] = 1.0;
                var imagePoint = _eCM * rp;

                Vector2 ip = new Vector2(imagePoint[0] / imagePoint[2], imagePoint[1] / imagePoint[2]);
                totalDiff += (ip - cp.Img).Length();
                Assert.IsTrue((ip - cp.Img).Length() < 1.0,
                    "Point after linear estimation too far : " + (ip - cp.Img).Length());
            }

            _calib.HomoPoints();
             _calib.NormalizeImagePoints();
             _calib.NormalizeRealPoints();
            _calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();
            _calib._pointsNormalised = true;
            _calib.FindNormalisedVariances();
            _calib.UseCovarianceMatrix = true;
            var lecm = _eCM.Clone();

            // Disturb camera matrix a little
              //          _calib.CameraMatrix = AddNoise(_calib.CameraMatrix);

            _calib._miniAlg.DoComputeJacobianNumerically = true;
            _calib._miniAlg.NumericalDerivativeStep = 1e-4;
            _calib.MinimizeError();

            // _calib.DenormaliseCameraMatrix();
            _calib.DecomposeCameraMatrix();
            _eCM = _calib.CameraMatrix;

            var errVec = _eCM.PointwiseDivide_NoNaN(lecm);
            double err = errVec.L2Norm();

            double scaleK = 1.0 / _calib.CameraInternalMatrix[2, 2];
            _eCM.MultiplyThis(-scaleK);

            var eK = _calib.CameraInternalMatrix.Multiply(scaleK);
            var eR = -_calib.CameraRotationMatrix;
            var eC = -(_eCM.SubMatrix(0, 3, 0, 3).Inverse() * _eCM.Column(3));

            Matrix<double> eExt = new DenseMatrix(3, 4);
            eExt.SetSubMatrix(0, 0, eR);
            eExt.SetColumn(3, -eR * eC);

            var eCM = eK * eExt;

            // var errVec = _CM.PointwiseDivide_NoNaN(_eCM);
            // double err = errVec.L2Norm();
            // Assert.IsTrue(
            //    Math.Abs(err - Math.Sqrt(12)) < Math.Sqrt(12) / 1000.0 || // max 0.1% diffrence
            //    (_eCM - _CM).FrobeniusNorm() < 1e-3);

            double estDiff = 0;
            for(int p = 0; p < _pointsCount; ++p)
            {
                var cp = points[p];
                Vector<double> rp = new DenseVector(4);
                rp[0] = cp.RealX;
                rp[1] = cp.RealY;
                rp[2] = cp.RealZ;
                rp[3] = 1.0;
                var imagePoint = _eCM * rp;

                Vector2 ip = new Vector2(imagePoint[0] / imagePoint[2], imagePoint[1] / imagePoint[2]);
                estDiff += (ip - cp.Img).Length();
                Assert.IsTrue((ip - cp.Img).Length() < 1.5,
                        "Point after error minimalisation too far : " + (ip - cp.Img).Length());
            }

            var minialg = _calib._miniAlg;
            // Test conovergence :
            // ||mX-rX|| = ||mX-eX|| + ||rX-eX|| (or squared??)
            // rX - real point from 'points'
            // mX - measured point, noised
            // eX = estimated X from result vector for 3d points and ePeX for image point
            double len2_mr = 0;
//.........这里部分代码省略.........

        public void PrepareCameraMatrix()
        {
            Matrix<double> K = new DenseMatrix(3);
            K[0, 0] = 10.0; // fx
            K[1, 1] = 10.0; // fy
            K[0, 1] = 0.0; // s
            K[0, 2] = 0.0; // x0
            K[1, 2] = 0.0; // y0
            K[2, 2] = 1.0; // 1

            Matrix<double> R = DenseMatrix.CreateIdentity(3);
            Vector<double> C = new DenseVector(3);
            C[0] = 50.0;
            C[1] = 50.0;
            C[2] = 0.0;

            Matrix<double> Ext = new DenseMatrix(3, 4);
            Ext.SetSubMatrix(0, 0, R);
            Ext.SetColumn(3, -R * C);

            _CM = K * Ext;
        }