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