public void LoadFromFile(Stream file, string path)
{
XmlDocument dataDoc = new XmlDocument();
dataDoc.Load(file);
_gridsList.Clear();
XmlNodeList grids = dataDoc.GetElementsByTagName("Grid");
foreach (XmlNode gridNode in grids)
{
RealGridData grid = new RealGridData();
//var gridNum = gridNode.Attributes["num"];
//if (gridNum != null)
// grid.Num = int.Parse(gridNum.Value);
//var gridLabel = gridNode.Attributes["label"];
//if (gridLabel != null)
// grid.Label = gridLabel.Value;
//XmlNode widthNode = gridNode.SelectSingleNode("child::Width");
// var wx = widthNode.Attributes["X"];
// grid.WidthX = double.Parse(wx.Value);
//var wy = widthNode.Attributes["Y"];
//grid.WidthY = double.Parse(wy.Value);
//var wz = widthNode.Attributes["Z"];
//grid.WidthZ = double.Parse(wz.Value);
//XmlNode heightNode = gridNode.SelectSingleNode("child::Height");
//var hx = heightNode.Attributes["X"];
//grid.HeightX = double.Parse(hx.Value);
//var hy = heightNode.Attributes["Y"];
//grid.HeightY = double.Parse(hy.Value);
//var hz = heightNode.Attributes["Z"];
//grid.HeightZ = double.Parse(hz.Value);
//XmlNode zeroNode = gridNode.SelectSingleNode("child::Zero");
//var zx = zeroNode.Attributes["X"];
//grid.ZeroX = double.Parse(zx.Value);
//var zy = zeroNode.Attributes["Y"];
//grid.ZeroY = double.Parse(zy.Value);
//var zz = zeroNode.Attributes["Z"];
//grid.ZeroZ = double.Parse(zz.Value);
_gridsList.Add(grid);
}
}
public void LoadFromFile(Stream file, string path)
{
XmlDocument dataDoc = new XmlDocument();
dataDoc.Load(file);
_gridsList.Clear();
XmlNodeList grids = dataDoc.GetElementsByTagName("Grid");
foreach (XmlNode gridNode in grids)
{
RealGridData grid = new RealGridData();
var gridNum = gridNode.Attributes["num"];
if (gridNum != null)
grid.Num = int.Parse(gridNum.Value);
var gridLabel = gridNode.Attributes["label"];
if (gridLabel != null)
grid.Label = gridLabel.Value;
XmlNode topleftNode = gridNode.SelectSingleNode("child::TopLeft");
grid.TopLeft = Vector3.CreateFromXmlNode(topleftNode);
XmlNode toprightNode = gridNode.SelectSingleNode("child::TopRight");
grid.TopRight = Vector3.CreateFromXmlNode(toprightNode);
XmlNode botleftNode = gridNode.SelectSingleNode("child::BotLeft");
grid.BotLeft = Vector3.CreateFromXmlNode(botleftNode);
XmlNode botrightNode = gridNode.SelectSingleNode("child::BotRight");
grid.BotRight = Vector3.CreateFromXmlNode(botrightNode);
var rowsNode = gridNode.SelectSingleNode("child::Rows");
if(rowsNode != null)
grid.Rows = int.Parse(rowsNode.Attributes["count"].Value);
var colsNode = gridNode.SelectSingleNode("child::Columns");
if(colsNode != null)
grid.Columns = int.Parse(colsNode.Attributes["count"].Value);
grid.Update();
_gridsList.Add(grid);
}
}
private void AddGrid(object sender, RoutedEventArgs e)
{
RealGridData rgrid = new RealGridData();
rgrid.Num = _gridsList.Count;
_gridsList.Add(rgrid);
//ListBoxItem item = new ListBoxItem();
//item.Content = rgrid;
//_gridListView.Items.Add(item);
}
private void UpdateRealPoints(List<CalibrationPoint> cpoints, RealGridData[] grids, Vector<double>[] reals)
{
for(int i = 0; i < CalibPointsLeft.Count; ++i)
{
var cp = cpoints[i];
var grid = grids[cp.GridNum];
var real = grid.GetRealFromCell(cp.RealRow, cp.RealCol);
reals[i] = new DenseVector(new double[4] { real.X, real.Y, real.Z, 1.0 });
}
}
private void UpdateGrids(RealGridData[] grids, int n0)
{
for(int i = 0; i < CalibGrids.Count; ++i)
{
int gridPos = 12 * i + n0;
grids[i].TopLeft.X = ResultsVector.At(gridPos);
grids[i].TopLeft.Y = ResultsVector.At(gridPos + 1);
grids[i].TopLeft.Z = ResultsVector.At(gridPos + 2);
grids[i].TopRight.X = ResultsVector.At(gridPos + 3);
grids[i].TopRight.Y = ResultsVector.At(gridPos + 4);
grids[i].TopRight.Z = ResultsVector.At(gridPos + 5);
grids[i].BotLeft.X = ResultsVector.At(gridPos + 6);
grids[i].BotLeft.Y = ResultsVector.At(gridPos + 7);
grids[i].BotLeft.Z = ResultsVector.At(gridPos + 8);
grids[i].BotRight.X = ResultsVector.At(gridPos + 9);
grids[i].BotRight.Y = ResultsVector.At(gridPos + 10);
grids[i].BotRight.Z = ResultsVector.At(gridPos + 11);
grids[i].Update();
}
}
public void SetGridError(Vector<double> error, RealGridData gridEst, RealGridData gridBase, int n0)
{
error.At(n0, _coeffGrids * (gridEst.TopLeft.X - gridBase.TopLeft.X));
error.At(n0 + 1, _coeffGrids * (gridEst.TopLeft.Y - gridBase.TopLeft.Y));
error.At(n0 + 2, _coeffGrids * (gridEst.TopLeft.Z - gridBase.TopLeft.Z));
error.At(n0 + 3, _coeffGrids * (gridEst.TopRight.X - gridBase.TopRight.X));
error.At(n0 + 4, _coeffGrids * (gridEst.TopRight.Y - gridBase.TopRight.Y));
error.At(n0 + 5, _coeffGrids * (gridEst.TopRight.Z - gridBase.TopRight.Z));
error.At(n0 + 6, _coeffGrids * (gridEst.BotLeft.X - gridBase.BotLeft.X));
error.At(n0 + 7, _coeffGrids * (gridEst.BotLeft.Y - gridBase.BotLeft.Y));
error.At(n0 + 8, _coeffGrids * (gridEst.BotLeft.Z - gridBase.BotLeft.Z));
error.At(n0 + 9, _coeffGrids * (gridEst.BotRight.X - gridBase.BotRight.X));
error.At(n0 + 10, _coeffGrids * (gridEst.BotRight.Y - gridBase.BotRight.Y));
error.At(n0 + 11, _coeffGrids * (gridEst.BotRight.Z - gridBase.BotRight.Z));
}
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 void MinimizeError()
{
PrepareMinimalisationAlg();
_miniAlg.Process();
// P = [pi | eXr]
var estimatedParams = _miniAlg.BestResultVector;
CameraMatrix[0, 0] = estimatedParams[0];
CameraMatrix[0, 1] = estimatedParams[1];
CameraMatrix[0, 2] = estimatedParams[2];
CameraMatrix[0, 3] = estimatedParams[3];
CameraMatrix[1, 0] = estimatedParams[4];
CameraMatrix[1, 1] = estimatedParams[5];
CameraMatrix[1, 2] = estimatedParams[6];
CameraMatrix[1, 3] = estimatedParams[7];
CameraMatrix[2, 0] = estimatedParams[8];
CameraMatrix[2, 1] = estimatedParams[9];
CameraMatrix[2, 2] = estimatedParams[10];
CameraMatrix[2, 3] = estimatedParams[11];
GridsEstimated = new List<RealGridData>(Grids.Count);
for(int i = 0; i < Grids.Count; ++i)
{
RealGridData eg = new RealGridData();
eg.TopLeft.X = estimatedParams.At(12 + i * 12);
eg.TopLeft.Y = estimatedParams.At(12 + i * 12 + 1);
eg.TopLeft.Z = estimatedParams.At(12 + i * 12 + 2);
eg.TopRight.X = estimatedParams.At(12 + i * 12 + 3);
eg.TopRight.Y = estimatedParams.At(12 + i * 12 + 4);
eg.TopRight.Z = estimatedParams.At(12 + i * 12 + 5);
eg.BotLeft.X = estimatedParams.At(12 + i * 12 + 6);
eg.BotLeft.Y = estimatedParams.At(12 + i * 12 + 7);
eg.BotLeft.Z = estimatedParams.At(12 + i * 12 + 8);
eg.BotRight.X = estimatedParams.At(12 + i * 12 + 9);
eg.BotRight.Y = estimatedParams.At(12 + i * 12 + 10);
eg.BotRight.Z = estimatedParams.At(12 + i * 12 + 11);
GridsEstimated.Add(eg);
}
}
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);
}
}
