/// <summary> /// Get the 1D Laplacian matrix (with Dirichlet boundary conditions). /// </summary> /// <param name="nx">Grid size.</param> /// <param name="eigenvalues">Vector to store eigenvalues (optional).</param> /// <returns>Laplacian sparse matrix.</returns> public static SparseMatrix Laplacian(int nx, DenseVector eigenvalues = null) { if (nx == 1) { // Handle special case n = 1. var A = new SparseMatrix(nx, nx); A.At(0, 0, 2.0); return(A); } var C = new CoordinateStorage <double>(nx, nx); for (int i = 0; i < nx; i++) { C.At(i, i, 2.0); if (i == 0) { C.At(i, i + 1, -1.0); } else if (i == (nx - 1)) { C.At(i, i - 1, -1.0); } else { C.At(i, i - 1, -1.0); C.At(i, i + 1, -1.0); } } if (eigenvalues != null) { // Compute eigenvalues. int count = Math.Min(nx, eigenvalues.Count); var eigs = new double[nx]; for (int i = 0; i < count; i++) { eigs[i] = 4 * Math.Pow(Math.Sin((i + 1) * Math.PI / (2 * (nx + 1))), 2); } Array.Sort(eigs); for (int i = 0; i < count; ++i) { eigenvalues.At(i, eigs[i]); } } return((SparseMatrix)C.ToSparseMatrix()); }
/// <summary> /// Get the 2D Laplacian matrix (with Dirichlet boundary conditions). /// </summary> /// <param name="nx">Number of elements in x direction.</param> /// <param name="ny">Number of elements in y direction.</param> /// <param name="eigenvalues">Vector to store eigenvalues (optional).</param> /// <returns>Laplacian sparse matrix.</returns> public static SparseMatrix Laplacian(int nx, int ny, DenseVector eigenvalues = null) { var Ix = Eye(nx); var Iy = Eye(ny); var Dx = Laplacian(nx); var Dy = Laplacian(ny); if (eigenvalues != null) { // Compute eigenvalues. int count = Math.Min(nx * ny, eigenvalues.Count); int index = 0; var eigs = new double[nx * ny]; double ax, ay; for (int i = 0; i < nx; i++) { ax = 4 * Math.Pow(Math.Sin((i + 1) * Math.PI / (2 * (nx + 1))), 2); for (int j = 0; j < ny; j++) { ay = 4 * Math.Pow(Math.Sin((j + 1) * Math.PI / (2 * (ny + 1))), 2); eigs[index++] = ax + ay; } } Array.Sort(eigs); for (int i = 0; i < count; ++i) { eigenvalues.At(i, eigs[i]); } } return((SparseMatrix)(Kron(Iy, Dx).FastAdd(Kron(Dy, Ix)))); }
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); _triangulation.PointsRight = new List<Vector<double>>(CalibPointsLeft.Count); for(int i = 0; i < CalibPointsLeft.Count; ++i) { _triangulation.PointsLeft.Add(CalibPointsLeft[i].Img.ToMathNetVector3()); _triangulation.PointsRight.Add(CalibPointsRight[i].Img.ToMathNetVector3()); } UseCovarianceMatrix = false; DumpingMethodUsed = DumpingMethod.Multiplicative; UpdateAll(); _lam = 1e-3f; _lastResidiual = _currentResidiual; Solver = new SvdSolver(); }
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; 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)); } } }
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; } //DCM calculation Matrix Cbn = quat_to_DCM(q); //Gyro Angles Matrix angles = dcm2angle(Cbn); double Psi = (double) angles.At(0, 0); double Theta = (double) angles.At(0, 1); double Gamma = (double) angles.At(0, 2); //Acceleration Angles double ThetaAcc = (double)Math.Atan2(Ab.At(0, 0), Math.Sqrt(Ab.At(1, 0) * Ab.At(1, 0) + Ab.At(2, 0) * Ab.At(2, 0))); double GammaAcc = (double)-Math.Atan2(Ab.At(1, 0), Math.Sqrt(Ab.At(0, 0) * Ab.At(0, 0) + Ab.At(2, 0) * Ab.At(2, 0))); //Horizontal projection of magnetic field angles.At(0, 0, 0); angles.At(0, 1, ThetaAcc); angles.At(0, 2, GammaAcc); Matrix Cbh = angle2dcm(angles); Matrix Mh = Matrix_Mult(Matrix_Transpose(Cbh), Mb); //Magnetic Heading double PsiMgn = (double)(-Math.Atan2(Mh.At(1,0),Mh.At(0,0)) + Param.declination); //System matrix Matrix A = new DenseMatrix (15, 15, 0); Matrix I = new DiagonalMatrix (15, 15, 1); A.At(0, 3, 1); A.At(1, 4, 1); A.At(2, 5, 1); A.At(0, 6, 1); A.At(1, 7, 1); A.At(2, 8, 1); A.At(0, 9, Omegab_ib.At(1, 0)); A.At(0, 10, Omegab_ib.At(2, 0)); A.At(1, 11, Omegab_ib.At(0, 0)); A.At(1, 12, Omegab_ib.At(2, 0)); A.At(2, 13, Omegab_ib.At(0, 0)); A.At(2, 14, Omegab_ib.At(1, 0)); Matrix F = Matrix_Plus(I, Matrix_Const_Mult(A,dT)); //Measurment Matrix double dPsi = Psi - PsiMgn; if (dPsi > Math.PI) dPsi = (double)(dPsi - 2 * Math.PI); if (dPsi < -Math.PI) dPsi = (double)(dPsi + 2 * Math.PI); double dTheta = Theta - ThetaAcc; if (dTheta > Math.PI) dTheta = (double)(dTheta - 2 * Math.PI); if (dTheta < -Math.PI) dTheta = (double)(dTheta + 2 * Math.PI); double dGamma = Gamma - GammaAcc; if (dGamma > Math.PI) dGamma = (double)(dGamma - 2 * Math.PI); if (dGamma < -Math.PI) dGamma = (double)(dGamma + 2 * Math.PI); Matrix z = new DenseMatrix(3, 1, 1); z.At(0, 0, dGamma); z.At(1, 0, dTheta); z.At(2, 0, dPsi); Matrix H = new DenseMatrix(3, 15, 0); H.At(0, 0, 1); H.At(1, 1, 1); H.At(2, 2, 1); if (Math.Abs(Math.Sqrt(Ab.At(0, 0) + Ab.At(1, 0) + Ab.At(2, 0))) > Param.accl_threshold) { H.At(0, 0, 0); H.At(1, 1, 0); } //Kalman Filter Matrix Q = State.Q; Matrix R = State.R; P = Matrix_Plus(Matrix_Mult(Matrix_Mult(F, P), Matrix_Transpose(F)),Q); if (P.At(0, 0).ToString() == "NaN") { P = new DiagonalMatrix(15, 15, (double)Math.Pow(10, -8)); P.At(0, 0, (double)Math.Pow(10, -1)); P.At(1, 1, (double)Math.Pow(10, -1)); P.At(2, 2, (double)Math.Pow(10, -1)); P.At(3, 3, (double)Math.Pow(10, -3)); P.At(4, 4, (double)Math.Pow(10, -3)); P.At(5, 5, (double)Math.Pow(10, -3)); restart = true; } Tuple<Matrix, Matrix> KF_result; KF_result = KF_Cholesky_update(P,z,R,H); Matrix xf = KF_result.Item1; P = KF_result.Item2; Matrix df_hat = new DenseMatrix(3, 1, 0); df_hat.At(0, 0, xf.At(0, 0)); df_hat.At(1, 0, xf.At(1, 0)); df_hat.At(2, 0, xf.At(2, 0)); Matrix dw_hat = new DenseMatrix(3, 1, 0); dw_hat.At(0, 0, xf.At(3, 0)); dw_hat.At(1, 0, xf.At(4, 0)); dw_hat.At(2, 0, xf.At(5, 0)); Matrix dB_hat = new DenseMatrix(3, 1, 0); dB_hat.At(0, 0, xf.At(6, 0)); dB_hat.At(1, 0, xf.At(7, 0)); dB_hat.At(2, 0, xf.At(8, 0)); Matrix dG_hat = new DenseMatrix(6, 1, 0); dG_hat.At(0, 0, xf.At(9, 0)); dG_hat.At(1, 0, xf.At(10, 0)); dG_hat.At(2, 0, xf.At(11, 0)); dG_hat.At(3, 0, xf.At(12, 0)); dG_hat.At(4, 0, xf.At(13, 0)); dG_hat.At(5, 0, xf.At(14, 0)); dw = Matrix_Plus(dw, dw_hat); dB = Matrix_Plus(dB, dB_hat); dG = Matrix_Plus(dG, dG_hat); Matrix dCbn = new DenseMatrix(3, 3, 0); dCbn.At(0, 1, -df_hat.At(2, 0)); dCbn.At(0, 2, df_hat.At(1, 0)); dCbn.At(1, 0, df_hat.At(2, 0)); dCbn.At(1, 2, -df_hat.At(0, 0)); dCbn.At(2, 0, -df_hat.At(1, 0)); dCbn.At(2, 1, df_hat.At(0, 0)); Cbn = Matrix_Mult(Matrix_Plus(new DiagonalMatrix(3, 3, 1), dCbn), Cbn); if (Cbn.At(0, 0).ToString() == "NaN") { restart = true; } if (dcm2quat(Cbn).At(0, 0).ToString() == "NaN") { restart = true; } q = dcm2quat(Cbn); if (q.At(0, 0).ToString() == "NaN") { restart = true; } q = quat_norm(q); if (q.At(0, 0).ToString() == "NaN") { restart = true; } Attitude.At(0, Psi); Attitude.At(1, Theta); Attitude.At(2, Gamma); Attitude.At(3, PsiMgn); Attitude.At(4, ThetaAcc); Attitude.At(5, GammaAcc); State.q = q; State.dG = dG; State.dB = dB; State.dw = dw; State.P = P; Sense.w = Matrix_Transpose(Omegab_ib); Sense.a = a; Sense.m = m; if (restart) { Matrix Initia_quat = new DenseMatrix(1, 4, 0); Initia_quat.At(0, 0, 1); State = new Kalman_class.State(ACCLERATION_NOISE, MAGNETIC_FIELD_NOISE, ANGULAR_VELOCITY_NOISE, Math.Pow(10, -6), Math.Pow(10, -15), Math.Pow(10, -15), Initia_quat); } return new Tuple <Vector, Sensors, State>(Attitude, Sense, State); }
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(); }