public override void Init()
{
int measuredPointsCount = (ParametersVector.Count - 12) / 3;
//int measuredPointsCount = (RealPoints.Count) / 3;
// Allocate matrices
_J = new DenseMatrix(MeasurementsVector.Count, ParametersVector.Count);
_Jt = new DenseMatrix(ParametersVector.Count, MeasurementsVector.Count);
_JtJ = new DenseMatrix(ParametersVector.Count, ParametersVector.Count);
_Jte = new DenseVector(ParametersVector.Count);
_currentErrorVector = new DenseVector(MeasurementsVector.Count);
_delta = new DenseVector(ParametersVector.Count);
ResultsVector = new DenseVector(ParametersVector.Count);
BestResultVector = new DenseVector(ParametersVector.Count);
ParametersVector.CopyTo(ResultsVector);
ParametersVector.CopyTo(BestResultVector);
_Lx = new DenseVector(measuredPointsCount);
_Ly = new DenseVector(measuredPointsCount);
_M = new DenseVector(measuredPointsCount);
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();
}
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();
}
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();
}