本文整理汇总了C#中MathNet.Numerics.LinearAlgebra.Double.DenseVector.At方法的典型用法代码示例。如果您正苦于以下问题:C# DenseVector.At方法的具体用法?C# DenseVector.At怎么用?C# DenseVector.At使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类MathNet.Numerics.LinearAlgebra.Double.DenseVector
的用法示例。
在下文中一共展示了DenseVector.At方法的7个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C#代码示例。
示例1: Init
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);
//.........这里部分代码省略.........
示例2: optimize
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;
//.........这里部分代码省略.........
示例3: AHRS_LKF_EULER
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;
}
//.........这里部分代码省略.........
示例4: getCoeffs
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;
}
示例5: NormalizeCalibGrids
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);
}
}
示例6: ComputeRectificationMatrices
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();
}
示例7: Init
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();
}