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