C# DenseVector.At方法代码示例

        public override void Init()
        {
            //ParametersVector = new DenseVector(24);
            //for(int i = 0; i < 12; ++i)
            //{
            //    ParametersVector.At(i, CameraLeft.At(i / 4, i & 3));
            //    ParametersVector.At(i + 12, CameraRight.At(i / 4, i & 3));
            //}

            // Parameters:
            // Full: [fx, fy, s, px, py, eaX, eaY, eaZ, Cx, Cy, Cz]
            // Center fixed: [fx, fy, s, eaX, eaY, eaZ, Cx, Cy, Cz]
            ParametersVector = new DenseVector(_cameraParamsCount * 2);
            if(_fxIdx >= 0) ParametersVector.At(_fxIdx, CalibrationData.Data.CalibrationLeft.At(0, 0));
            if(_fyIdx >= 0) ParametersVector.At(_fyIdx, CalibrationData.Data.CalibrationLeft.At(1, 1));
            if(_skIdx >= 0) ParametersVector.At(_skIdx, CalibrationData.Data.CalibrationLeft.At(0, 1));
            if(_pxIdx >= 0) ParametersVector.At(_pxIdx, CalibrationData.Data.CalibrationLeft.At(0, 2));
            if(_pyIdx >= 0) ParametersVector.At(_pyIdx, CalibrationData.Data.CalibrationLeft.At(1, 2));

            Vector<double> euler = new DenseVector(3);
            RotationConverter.MatrixToEuler(euler, CalibrationData.Data.RotationLeft);
            if(_euXIdx >= 0) ParametersVector.At(_euXIdx, euler.At(0));
            if(_euYIdx >= 0) ParametersVector.At(_euYIdx, euler.At(1));
            if(_euZIdx >= 0) ParametersVector.At(_euZIdx, euler.At(2));

            if(_cXIdx >= 0) ParametersVector.At(_cXIdx, CalibrationData.Data.TranslationLeft.At(0));
            if(_cYIdx >= 0) ParametersVector.At(_cYIdx, CalibrationData.Data.TranslationLeft.At(1));
            if(_cZIdx >= 0) ParametersVector.At(_cZIdx, CalibrationData.Data.TranslationLeft.At(2));

            int n0 = _cameraParamsCount;
            if(_fxIdx >= 0) ParametersVector.At(_fxIdx + n0, CalibrationData.Data.CalibrationRight.At(0, 0));
            if(_fyIdx >= 0) ParametersVector.At(_fyIdx + n0, CalibrationData.Data.CalibrationRight.At(1, 1));
            if(_skIdx >= 0) ParametersVector.At(_skIdx + n0, CalibrationData.Data.CalibrationRight.At(0, 1));
            if(_pxIdx >= 0) ParametersVector.At(_pxIdx + n0, CalibrationData.Data.CalibrationRight.At(0, 2));
            if(_pyIdx >= 0) ParametersVector.At(_pyIdx + n0, CalibrationData.Data.CalibrationRight.At(1, 2));

            RotationConverter.MatrixToEuler(euler, CalibrationData.Data.RotationRight);
            if(_euXIdx >= 0) ParametersVector.At(_euXIdx + n0, euler.At(0));
            if(_euYIdx >= 0) ParametersVector.At(_euYIdx + n0, euler.At(1));
            if(_euZIdx >= 0) ParametersVector.At(_euZIdx + n0, euler.At(2));

            if(_cXIdx >= 0) ParametersVector.At(_cXIdx + n0, CalibrationData.Data.TranslationRight.At(0));
            if(_cYIdx >= 0) ParametersVector.At(_cYIdx + n0, CalibrationData.Data.TranslationRight.At(1));
            if(_cZIdx >= 0) ParametersVector.At(_cZIdx + n0, CalibrationData.Data.TranslationRight.At(2));

            //_imgCenterLeft = new Vector2(CalibrationData.Data.CalibrationLeft.At(0, 2),
            //    CalibrationData.Data.CalibrationLeft.At(1, 2));
            //_imgCenterRight = new Vector2(CalibrationData.Data.CalibrationRight.At(0, 2),
            //    CalibrationData.Data.CalibrationRight.At(1, 2));

            ResultsVector = new DenseVector(ParametersVector.Count + CalibGrids.Count * 12);
            BestResultVector = new DenseVector(ResultsVector.Count);
            ParametersVector.CopySubVectorTo(ResultsVector, 0, 0, ParametersVector.Count);

            _grids = new RealGridData[CalibGrids.Count];

            int N = ParametersVector.Count;
            for(int i = 0; i < CalibGrids.Count; ++i)
            {
                ResultsVector.At(N + i * 12, CalibGrids[i].TopLeft.X);
                ResultsVector.At(N + i * 12 + 1, CalibGrids[i].TopLeft.Y);
                ResultsVector.At(N + i * 12 + 2, CalibGrids[i].TopLeft.Z);
                ResultsVector.At(N + i * 12 + 3, CalibGrids[i].TopRight.X);
                ResultsVector.At(N + i * 12 + 4, CalibGrids[i].TopRight.Y);
                ResultsVector.At(N + i * 12 + 5, CalibGrids[i].TopRight.Z);
                ResultsVector.At(N + i * 12 + 6, CalibGrids[i].BotLeft.X);
                ResultsVector.At(N + i * 12 + 7, CalibGrids[i].BotLeft.Y);
                ResultsVector.At(N + i * 12 + 8, CalibGrids[i].BotLeft.Z);
                ResultsVector.At(N + i * 12 + 9, CalibGrids[i].BotRight.X);
                ResultsVector.At(N + i * 12 + 10, CalibGrids[i].BotRight.Y);
                ResultsVector.At(N + i * 12 + 11, CalibGrids[i].BotRight.Z);
                _grids[i] = new RealGridData();
                _grids[i].Columns = CalibGrids[i].Columns;
                _grids[i].Rows = CalibGrids[i].Rows;
            }
            ResultsVector.CopyTo(BestResultVector);

            _coeffMatch = MatchedPointsLeft.Count > 0 ? Math.Sqrt(MatchErrorCoeff * 0.5 / (double)MatchedPointsLeft.Count) : 0;
            _coeffImages = Math.Sqrt(ImagesErrorCoeff * 0.5 / (double)(CalibPointsLeft.Count + CalibPointsRight.Count));
            _coeffGrids = Math.Sqrt(GridsErrorCoeff * (CalibPointsLeft.Count + CalibPointsRight.Count) / (double)(CalibGrids.Count * 12));
            _coeffTriang = Math.Sqrt(TriangulationErrorCoeff * 0.33 / (double)CalibPointsLeft.Count);

            _currentErrorVector = new DenseVector(
                MatchedPointsLeft.Count * 2 + // Matched
                CalibPointsLeft.Count * 3 + // Triangulation
                CalibPointsLeft.Count * 2 + CalibPointsRight.Count * 2 + // Image
                CalibGrids.Count * 12); // Grids

            _J = new DenseMatrix(_currentErrorVector.Count, ResultsVector.Count);
            _Jt = new DenseMatrix(ResultsVector.Count, _currentErrorVector.Count);
            _JtJ = new DenseMatrix(ResultsVector.Count, ResultsVector.Count);
            _Jte = new DenseVector(ResultsVector.Count);
            _delta = new DenseVector(ResultsVector.Count);

            _reals = new Vector<double>[CalibPointsLeft.Count];
            _imgsLeft = new Vector<double>[CalibPointsLeft.Count];
            _imgsRight = new Vector<double>[CalibPointsRight.Count];

            _triangulation.UseLinearEstimationOnly = true;
            _triangulation.PointsLeft = new List<Vector<double>>(CalibPointsLeft.Count);
//.........这里部分代码省略.........

        private void optimize(DenseMatrix coefficients, DenseVector objFunValues, bool artifical)
        {
            //for calculations on the optimal solution row
            int cCounter,
                width = coefficients.ColumnCount;
            DenseVector cBVect = new DenseVector(basics.Count);

            //Sets up the b matrix
            DenseMatrix b = new DenseMatrix(basics.Count, 1);

            //basics will have values greater than coefficients.ColumnCount - 1 if there are still artificial variables
            //or if Nathan is bad and didn't get rid of them correctly
            foreach (int index in basics)
            {
                b = (DenseMatrix)b.Append(DenseVector.OfVector(coefficients.Column(index)).ToColumnMatrix());
            }
            // removes the first column
            b = (DenseMatrix)b.SubMatrix(0, b.RowCount, 1, b.ColumnCount - 1);

            double[] cPrimes = new double[width];
            double[] rhsOverPPrime;
            DenseMatrix[] pPrimes = new DenseMatrix[width];
            DenseMatrix bInverse;

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

    public static Tuple<Vector, Sensors, State> AHRS_LKF_EULER(Sensors Sense, State State, Parameters Param)
    {
        Vector Attitude = new DenseVector(6, 0);
        bool restart = false;
        // get sensor data
        Matrix m = Sense.m;
        Matrix a = Sense.a;
        Matrix w = Sense.w;

        // Correct magntometers using callibration coefficients
        Matrix B = new DenseMatrix(3,3);
        B.At(0, 0, Param.magn_coefs.At(0));
        B.At(0, 1, Param.magn_coefs.At(3));
        B.At(0, 2, Param.magn_coefs.At(4));
        B.At(1, 0, Param.magn_coefs.At(5));
        B.At(1, 1, Param.magn_coefs.At(1));
        B.At(1, 2, Param.magn_coefs.At(6));
        B.At(2, 0, Param.magn_coefs.At(7));
        B.At(2, 1, Param.magn_coefs.At(8));
        B.At(2, 2, Param.magn_coefs.At(2));
        Matrix B0 = new DenseMatrix(3, 1);
        B0.At(0, 0, Param.magn_coefs.At(9));
        B0.At(1, 0, Param.magn_coefs.At(10));
        B0.At(2, 0, Param.magn_coefs.At(11));

        m = Matrix_Transpose(Matrix_Mult(Matrix_Minus(new DiagonalMatrix(3,3,1),B),Matrix_Minus(Matrix_Transpose(m),B0)));

        // Correct accelerometers using callibration coefficients
        B.At(0, 0, Param.accl_coefs.At(0));
        B.At(0, 1, Param.accl_coefs.At(3));
        B.At(0, 2, Param.accl_coefs.At(4));
        B.At(1, 0, Param.accl_coefs.At(5));
        B.At(1, 1, Param.accl_coefs.At(1));
        B.At(1, 2, Param.accl_coefs.At(6));
        B.At(2, 0, Param.accl_coefs.At(7));
        B.At(2, 1, Param.accl_coefs.At(8));
        B.At(2, 2, Param.accl_coefs.At(2));

        B0.At(0, 0, Param.accl_coefs.At(9));
        B0.At(1, 0, Param.accl_coefs.At(10));
        B0.At(2, 0, Param.accl_coefs.At(11));

        a = Matrix_Transpose(Matrix_Mult(Matrix_Minus(new DiagonalMatrix(3, 3, 1), B), Matrix_Minus(Matrix_Transpose(a), B0)));

        // Correct gyroscopes using callibration coefficients
        B.At(0, 0, Param.gyro_coefs.At(0));
        B.At(0, 1, Param.gyro_coefs.At(3));
        B.At(0, 2, Param.gyro_coefs.At(4));
        B.At(1, 0, Param.gyro_coefs.At(5));
        B.At(1, 1, Param.gyro_coefs.At(1));
        B.At(1, 2, Param.gyro_coefs.At(6));
        B.At(2, 0, Param.gyro_coefs.At(7));
        B.At(2, 1, Param.gyro_coefs.At(8));
        B.At(2, 2, Param.gyro_coefs.At(2));

        B0.At(0, 0, Param.gyro_coefs.At(9));
        B0.At(1, 0, Param.gyro_coefs.At(10));
        B0.At(2, 0, Param.gyro_coefs.At(11));

        w = Matrix_Transpose(Matrix_Mult(Matrix_Minus(new DiagonalMatrix(3, 3, 1), B), Matrix_Minus(Matrix_Transpose(w), B0)));

        // Get State
        Matrix q = State.q;
        Matrix dB = State.dB;
        Matrix dG = State.dG;
        Matrix dw = State.dw;
        Matrix P = State.P;

        Matrix Wb = Matrix_Transpose(w);
        Matrix Ab = Matrix_Transpose(a);
        Matrix Mb = Matrix_Transpose(m);

        double dT = Param.dT;

        //Correct Gyroscopes for estimate biases and scale factor
        B.At(0, 0, dB.At(0, 0));
        B.At(0, 1, dG.At(0, 0));
        B.At(0, 2, dG.At(1, 0));
        B.At(1, 0, dG.At(2, 0));
        B.At(1, 1, dB.At(1, 0));
        B.At(1, 2, dG.At(3, 0));
        B.At(2, 0, dG.At(4, 0));
        B.At(2, 1, dG.At(5, 0));
        B.At(2, 2, dB.At(2, 0));

        Matrix Omegab_ib;
        Omegab_ib = Matrix_Minus(Matrix_Mult(Matrix_Minus(new DiagonalMatrix(3, 3, 1), B), Wb),dw);

        if (q.At(0, 0).ToString() == "NaN")
        {
            restart = true;
        }

        //Quternion calculation
        q = mrotate(q, Omegab_ib, dT);

        if (q.At(0,0).ToString() == "NaN")
        {
            restart = true;
        }
//.........这里部分代码省略.........

        private double[] getCoeffs(bool includeLinear)
        {
            double[] coeffs = null;
              try
              {

            DenseMatrix X = new DenseMatrix(_calibrationData.Count, 3);
            DenseVector Y = new DenseVector(_calibrationData.Count);
            int i = 0;
            foreach (Spot _spot in _calibrationData)
            {
              X.At(i, 0, includeLinear ? _spot.s : 0);
              X.At(i, 1, _spot.s * _spot.s);
              X.At(i, 2, _spot.s * _spot.s * _spot.s);
              Y.At(i, _spot.p);
              i++;
            }
            var XT = X.Transpose();
            var A = (XT * X).Inverse() * XT * Y;
            coeffs = A.ToArray();
              }
              catch (Exception ex)
              {
            coeffs = null;
              }
              return coeffs;
        }

        public void NormalizeCalibGrids()
        {
            GridsNormalised = new List<RealGridData>();
            for(int i = 0; i < Grids.Count; ++i)
            {
                RealGridData grid = Grids[i];
                RealGridData gridNorm = new RealGridData();
                gridNorm.Rows = grid.Rows;
                gridNorm.Columns = grid.Columns;

                Vector<double> corner = new DenseVector(new double[] { grid.TopLeft.X, grid.TopLeft.Y, grid.TopLeft.Z, 1.0 });
                corner = NormReal * corner;
                corner.DivideThis(corner.At(3));
                gridNorm.TopLeft = new Vector3(corner.At(0), corner.At(1), corner.At(2));

                corner = new DenseVector(new double[] { grid.TopRight.X, grid.TopRight.Y, grid.TopRight.Z, 1.0 });
                corner = NormReal * corner;
                corner.DivideThis(corner.At(3));
                gridNorm.TopRight = new Vector3(corner.At(0), corner.At(1), corner.At(2));

                corner = new DenseVector(new double[] { grid.BotLeft.X, grid.BotLeft.Y, grid.BotLeft.Z, 1.0 });
                corner = NormReal * corner;
                corner.DivideThis(corner.At(3));
                gridNorm.BotLeft = new Vector3(corner.At(0), corner.At(1), corner.At(2));

                corner = new DenseVector(new double[] { grid.BotRight.X, grid.BotRight.Y, grid.BotRight.Z, 1.0 });
                corner = NormReal * corner;
                corner.DivideThis(corner.At(3));
                gridNorm.BotRight = new Vector3(corner.At(0), corner.At(1), corner.At(2));

                gridNorm.Update();
                GridsNormalised.Add(gridNorm);
            }
        }

        public override void ComputeRectificationMatrices()
        {
            _minimalisation = new Minimalisation();
            _minimalisation.MinimumResidiual = 1e-12;
            _minimalisation.MaximumIterations = 1000;
            _minimalisation.DoComputeJacobianNumerically = true;
            _minimalisation.DumpingMethodUsed = LevenbergMarquardtBaseAlgorithm.DumpingMethod.Multiplicative;
            _minimalisation.UseCovarianceMatrix = false;
            _minimalisation.NumericalDerivativeStep = 1e-4;

            _minimalisation.ParametersVector = new DenseVector(Minimalisation._paramsCount);
            if(UseInitialCalibration && CalibrationData.Data.IsCamLeftCalibrated && CalibrationData.Data.IsCamRightCalibrated)
            {
                _minimalisation.ParametersVector.At(Minimalisation._flIdx,
                    0.5 * (CalibrationData.Data.CalibrationLeft.At(0, 0) +
                    CalibrationData.Data.CalibrationLeft.At(1, 1)));
                // _minimalisation.ParametersVector.At(Minimalisation._pxlIdx,
                //     CalibrationData.Data.CalibrationLeft.At(0, 2));
                // _minimalisation.ParametersVector.At(Minimalisation._pylIdx,
                //     CalibrationData.Data.CalibrationLeft.At(1, 2));

                Vector<double> euler = new DenseVector(3);
                RotationConverter.MatrixToEuler(euler, CalibrationData.Data.RotationLeft);
                _minimalisation.ParametersVector.At(Minimalisation._eYlIdx, euler.At(1));
                _minimalisation.ParametersVector.At(Minimalisation._eZlIdx, euler.At(2));

                _minimalisation.ParametersVector.At(Minimalisation._frIdx,
                    0.5 * (CalibrationData.Data.CalibrationRight.At(0, 0) +
                    CalibrationData.Data.CalibrationRight.At(1, 1)));
                // _minimalisation.ParametersVector.At(Minimalisation._pxrIdx,
                //     CalibrationData.Data.CalibrationRight.At(0, 2));
                // _minimalisation.ParametersVector.At(Minimalisation._pyrIdx,
                //     CalibrationData.Data.CalibrationRight.At(1, 2));

                RotationConverter.MatrixToEuler(euler, CalibrationData.Data.RotationRight);
                _minimalisation.ParametersVector.At(Minimalisation._eXrIdx, euler.At(0));
                _minimalisation.ParametersVector.At(Minimalisation._eYrIdx, euler.At(1));
                _minimalisation.ParametersVector.At(Minimalisation._eZrIdx, euler.At(2));
            }
            else
            {
                // Some other normalisation:
                // let unit length be (imgwidth + imgheight)
                // then fx0 = 1, px0 = w/(w+h), py0 = h/(w+h)
                // Also limit angle range to -+pi

                _minimalisation.ParametersVector.At(Minimalisation._flIdx, 1.0);
                _minimalisation.ParametersVector.At(Minimalisation._pxlIdx, 0.5 * (double)ImageWidth / (double)(ImageWidth + ImageHeight));
                _minimalisation.ParametersVector.At(Minimalisation._pylIdx, 0.5 * (double)ImageHeight / (double)(ImageWidth + ImageHeight));
                //_minimalisation.ParametersVector.At(Minimalisation._flIdx, ImageWidth + ImageHeight);
                //_minimalisation.ParametersVector.At(Minimalisation._pxlIdx, ImageWidth / 2);
                //_minimalisation.ParametersVector.At(Minimalisation._pylIdx, ImageHeight / 2);

                _minimalisation.ParametersVector.At(Minimalisation._eYlIdx, 0.0);
                _minimalisation.ParametersVector.At(Minimalisation._eZlIdx, 0.0);

                _minimalisation.ParametersVector.At(Minimalisation._frIdx, 1.0);
                _minimalisation.ParametersVector.At(Minimalisation._pxrIdx, 0.5 * (double)ImageWidth / (double)(ImageWidth + ImageHeight));
                _minimalisation.ParametersVector.At(Minimalisation._pyrIdx, 0.5 * (double)ImageHeight / (double)(ImageWidth + ImageHeight));
                //_minimalisation.ParametersVector.At(Minimalisation._frIdx, ImageWidth + ImageHeight);
                //_minimalisation.ParametersVector.At(Minimalisation._pxrIdx, ImageWidth / 2);
                //_minimalisation.ParametersVector.At(Minimalisation._pyrIdx, ImageHeight / 2);

                _minimalisation.ParametersVector.At(Minimalisation._eXrIdx, 0.0);
                _minimalisation.ParametersVector.At(Minimalisation._eYrIdx, 0.0);
                _minimalisation.ParametersVector.At(Minimalisation._eZrIdx, 0.0);
            }

            _minimalisation.PointsLeft = new List<Vector<double>>();
            _minimalisation.PointsRight = new List<Vector<double>>();
            for(int i = 0; i < PointsLeft.Count; ++i)
            {
                _minimalisation.PointsLeft.Add(PointsLeft[i].ToMathNetVector3());
                _minimalisation.PointsRight.Add(PointsRight[i].ToMathNetVector3());
            }

            _minimalisation.ImageHeight = ImageHeight;
            _minimalisation.ImageWidth = ImageWidth;
            _minimalisation.Process();

            //_minimalisation.Init();
            _minimalisation.BestResultVector.CopyTo(_minimalisation.ResultsVector);
            _minimalisation.UpdateAll();

            Matrix<double> halfRevolve = new DenseMatrix(3, 3);
            RotationConverter.EulerToMatrix(new double[] { 0.0, 0.0, Math.PI }, halfRevolve);

            Matrix<double> K = (_minimalisation._Kol + _minimalisation._Kor).Multiply(0.5);

            _H_L = K * _minimalisation._Rl * (_minimalisation._Kol.Inverse());
            _H_R = K * _minimalisation._Rr * (_minimalisation._Kor.Inverse());

            ComputeScalingMatrices(ImageWidth, ImageHeight);

            RectificationLeft = _Ht_L * _H_L;
            RectificationRight = _Ht_R * _H_R;

            RectificationLeft_Inverse = RectificationLeft.Inverse();
            RectificationRight_Inverse = RectificationRight.Inverse();
        }

        public override void Init()
        {
            ResultsVector = new DenseVector(ParametersVector.Count + CalibrationGrids.Count * 12);
            BestResultVector = new DenseVector(ParametersVector.Count + CalibrationGrids.Count * 12);
            ParametersVector.CopySubVectorTo(ResultsVector, 0, 0, 12);
            for(int i = 0; i < CalibrationGrids.Count; ++i)
            {
                ResultsVector.At(12 + i * 12, CalibrationGrids[i].TopLeft.X);
                ResultsVector.At(12 + i * 12 + 1, CalibrationGrids[i].TopLeft.Y);
                ResultsVector.At(12 + i * 12 + 2, CalibrationGrids[i].TopLeft.Z);
                ResultsVector.At(12 + i * 12 + 3, CalibrationGrids[i].TopRight.X);
                ResultsVector.At(12 + i * 12 + 4, CalibrationGrids[i].TopRight.Y);
                ResultsVector.At(12 + i * 12 + 5, CalibrationGrids[i].TopRight.Z);
                ResultsVector.At(12 + i * 12 + 6, CalibrationGrids[i].BotLeft.X);
                ResultsVector.At(12 + i * 12 + 7, CalibrationGrids[i].BotLeft.Y);
                ResultsVector.At(12 + i * 12 + 8, CalibrationGrids[i].BotLeft.Z);
                ResultsVector.At(12 + i * 12 + 9, CalibrationGrids[i].BotRight.X);
                ResultsVector.At(12 + i * 12 + 10, CalibrationGrids[i].BotRight.Y);
                ResultsVector.At(12 + i * 12 + 11, CalibrationGrids[i].BotRight.Z);
            }
            ResultsVector.CopyTo(BestResultVector);
            _grids = new List<RealGridData>(CalibrationGrids);
            _gridErrorCoef = Math.Sqrt((double)CalibrationPoints.Count / (double)(CalibrationGrids.Count * 12.0));

            _currentErrorVector = new DenseVector(CalibrationGrids.Count * 12 + CalibrationPoints.Count * 2);
            _J = new DenseMatrix(_currentErrorVector.Count, ResultsVector.Count);
            _Jt = new DenseMatrix(ResultsVector.Count, _currentErrorVector.Count);
            _JtJ = new DenseMatrix(ResultsVector.Count, ResultsVector.Count);
            _Jte = new DenseVector(ResultsVector.Count);
            _delta = new DenseVector(ResultsVector.Count);
            _Lx = new DenseVector(CalibrationPoints.Count);
            _Ly = new DenseVector(CalibrationPoints.Count);
            _M = new DenseVector(CalibrationPoints.Count);
            _reals = new Vector3[CalibrationPoints.Count];

            UpdateAll();

            //if(DumpingMethodUsed == DumpingMethod.Additive)
            //{
            //    // Compute initial lambda lam = 10^-3*diag(J'J)/size(J'J)
            //    ComputeJacobian(_J);
            //    _J.TransposeToOther(_Jt);
            //    _Jt.MultiplyToOther(_J, _JtJ);
            //    _lam = 1e-3f * _JtJ.Trace() / (double)_JtJ.ColumnCount;
            //}
            //else
            if(DumpingMethodUsed == DumpingMethod.Multiplicative)
            {
                _lam = 1e-3f;
            }
            else
                _lam = 0.0;

            _lastResidiual = _currentResidiual;
            Solver = new SvdSolver();
        }