C# DenseMatrix.SetSubMatrix方法代码示例

        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;
        }

        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;
        }

//.........这里部分代码省略.........
                {
                    c.SetColumn(f, c.Column(f) * generator.NextDouble());
                }
            }

            // todo:
            // generator.shuffle(C)

            // Loop over all clusters
            int pos = 0;
            int posEnd = 0;

            for (int k = 0; k < nClusters; k++)
            {
                // Number of samples in cluster k
                int nSamplesK = nSamplesPerCluster[k];

                // Define the range of samples
                pos = posEnd;
                posEnd = pos + nSamplesK;

                // Assign labels
                for (int l = pos; l < posEnd; l++)
                {
                    y[l] = k % nClasses;
                }

                // Draw features at random
                var subMatrix = DenseMatrix.CreateRandom(
                    nSamplesK,
                    nInformative,
                    new Normal { RandomSource = generator });

                x.SetSubMatrix(pos, nSamplesK, 0, nInformative, subMatrix);

                // Multiply by a random matrix to create co-variance of the features
                var uniform = new ContinuousUniform(-1, 1) { RandomSource = generator };
                Matrix a = DenseMatrix.CreateRandom(nInformative, nInformative, uniform);

                x.SetSubMatrix(
                    pos,
                    nSamplesK,
                    0,
                    nInformative,
                    x.SubMatrix(pos, nSamplesK, 0, nInformative) * a);

                // Shift the cluster to a vertice
                var v = x.SubMatrix(pos, nSamplesK, 0, nInformative).AddRowVector(c.Row(k));
                x.SetSubMatrix(pos, nSamplesK, 0, nInformative, v);
            }

            // Create redundant features
            if (nRedundant > 0)
            {
                var uniform = new ContinuousUniform(-1, 1) { RandomSource = generator };
                Matrix b = DenseMatrix.CreateRandom(nInformative, nRedundant, uniform);
                x.SetSubMatrix(
                    0,
                    x.RowCount,
                    nInformative,
                    nRedundant,
                    x.SubMatrix(0, x.RowCount, 0, nInformative) * b);
            }

            // Repeat some features
            if (nRepeated > 0)

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

        /// <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));
//.........这里部分代码省略.........

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

            // Шаг 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;
                }
            }
            newRow[falseIndex] = 0;
            _writer.WriteLine("New limitation: {0}", newRow);

            var newb = (y * _task.b);   // Находим новый элемент вектора b
            newb = newb > 0 ? -(newb - Math.Floor(newb)) : -(Math.Ceiling(Math.Abs(newb)) - Math.Abs(newb)); // TODO probably need to rewrite this

            _writer.WriteLine("New b = {0}", newb);

            // Шаг 5
            var newMatrix = new DenseMatrix(_task.A.RowCount + 1, _task.A.ColumnCount + 1); // Формируем новую
            newMatrix.SetSubMatrix(0, _task.A.RowCount, 0, _task.A.ColumnCount, _task.A);   // матрицу А
            newMatrix.SetRow(_task.A.RowCount, newRow);
            newMatrix[_task.A.RowCount, _task.A.ColumnCount] = 1;

            var newBVector = new DenseVector(_task.b.Count + 1);    // Формируем новый
            newBVector.SetSubVector(0, _task.b.Count, _task.b);     // вектор b
            newBVector[_task.b.Count] = newb;

            var newCVector = new DenseVector(_task.c.Count + 1);    // Добавляем новую
            newCVector.SetSubVector(0, _task.c.Count, _task.c);     // компоненту вектора с

            var newJb = _task.Jb.ToList();
            newJb.Add(newJb[newJb.Count - 1] + 1);
            _artJ.Add(new ArtJEntry { Column = newMatrix.ColumnCount - 1, Row = newMatrix.RowCount - 1 });

            _task.A = newMatrix.Clone();        // Создаем
            _task.b = newBVector.Clone();       // новую задачу
            _task.c = newCVector.Clone();       // для следующей итерации
            _task.Jb = newJb;

            iterationNumber++;              // Присваиваем новый номер итерации

            return true;
        }

 /// <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;
 }

        public static ResultFEM Solve(Matrix<double> K, Matrix<double> f, int[] bc)
        {
            // Create dof array:
            int ndof = f.RowCount;

            // Initialize displacement vector
            Matrix u = new DenseMatrix(ndof, 1);

            int[] dof = new int[ndof];
            for (int i = 0; i < ndof; i++)
            {
                dof[i] = i;
            }
            // Create the free dofs:
            int[] fdof = dof.Except(bc).ToArray();

            int nfdof = fdof.Length; // Number of free dofs.
            int nbdof = ndof - nfdof; //Constrained dofs

            // Create the permutation array which puts the constrained dofs last:
            int[] permute = bc.Union(fdof).ToArray();

            Permutation perm = new Permutation(permute);
            Permutation invPerm = perm.Inverse();
            Matrix<double> KPermuted = DenseMatrix.OfMatrix(K);
            KPermuted.PermuteRows(invPerm);
            KPermuted.PermuteColumns(invPerm);

            // Split K:::
            Matrix<double> Kff = KPermuted.SubMatrix(nbdof, nfdof, nbdof, nfdof);
            System.Console.WriteLine(Kff);  //Down right corner of matrix
            Matrix<double> Kfp = KPermuted.SubMatrix(nbdof, nfdof, 0, nbdof); //Down left corner of matrix

            // Split F:::
            Matrix<double> fPermuted = DenseMatrix.OfMatrix(f);
            fPermuted.PermuteRows(invPerm);
            Matrix<double> ff = fPermuted.SubMatrix(nbdof, nfdof, 0, 1);

            Matrix<double> up = u.SubMatrix(0, nbdof, 0, 1); // Must set up to constrained values in bc.

            // Solve for the unknown displacements:::
            Matrix<double> s = Kff.Solve(ff.Subtract(Kfp.Multiply(up)));
            u.SetSubMatrix(nbdof, 0, s); // Set displacements back.

            System.Console.WriteLine(u);
            // Permute back u
            u.PermuteRows(perm);

            System.Console.WriteLine("U after permut");
            System.Console.WriteLine(K);
            System.Console.WriteLine(u);
            System.Console.WriteLine(f);

            // Get reaction forces:
            Matrix<double> Q = K.Multiply(u).Subtract(f);

            ResultFEM result = new ResultFEM(u, Q);

            return result;
        }

        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;
        }