public static unsafe void ceresSolveAruco() { var phc = PinholeCamera.getTestCameraHuawei(); string dir = @"C:\Users\jens\Desktop\calibratie\Huawei p9\aruco\stereo test\"; List <CeresMarker> ceresmarkers = new List <CeresMarker>(); List <CeresCamera> cerescameras = new List <CeresCamera>(); var files = Directory.GetFiles(dir).ToList(); //8 punten nodig var markerDictionary = Aruco.findArucoMarkers(files, Path.Combine(dir, "aruco_detected\\"), 1); var pairs = findImagePairsMinMarkers(markerDictionary, 8); Matrix K = new Matrix(phc.Intrinsics.Mat); var W = new Matrix(new double[] { 0.0D, -1.0D, 0.0D, 1.0D, 0.0D, 0.0D, 0.0D, 0.0D, 1.0D }); var Wt = new Matrix(new double[] { 0.0D, 1.0D, 0.0D, -1.0D, 0.0D, 0.0D, 0.0D, 0.0D, 1.0D }); var Z = new Matrix(new double[] { 0.0D, 1.0D, 0.0D, -1.0D, 0.0D, 0.0D, 0.0D, 0.0D, 0D }); var diag = new Matrix(new double[] { 1.0D, 0.0D, 0.0D, 0.0D, 1.0D, 0.0D, 0.0D, 0.0D, 0.0D }); foreach (var stereoPair in pairs) { var points_count = stereoPair.intersection.Count; VectorOfPointF punten1px, punten2px; { int i = 0; List <PointF> p1 = new List <PointF>(); List <PointF> p2 = new List <PointF>(); foreach (KeyValuePair <ArucoMarker, ArucoMarker> kvp in stereoPair.intersection) { p1.Add(kvp.Key.Corner1); p2.Add(kvp.Value.Corner1); i++; } punten1px = new VectorOfPointF(p1.ToArray()); punten2px = new VectorOfPointF(p2.ToArray()); } Matrix F = new Matrix(3, 3); CVI.FindFundamentalMat(punten1px, punten2px, F); Matrix essential = K.Transpose() * F * K; var decomp = new SVD <double>(essential); var U = decomp.U; var Vt = decomp.Vt; var R1 = U * W * Vt; var R2 = U * W.Transpose() * Vt; var T1 = U.GetCol(2); var T2 = -1 * U.GetCol(2); Matrix[] Ps = new Matrix[4]; for (int i = 0; i < 4; i++) { Ps[i] = new Matrix(3, 4); } CVI.HConcat(R1, T1, Ps[0]); CVI.HConcat(R1, T2, Ps[1]); CVI.HConcat(R2, T1, Ps[2]); CVI.HConcat(R2, T2, Ps[3]); var KPs = new Matrix[4]; KPs[0] = K * Ps[0]; KPs[1] = K * Ps[1]; KPs[2] = K * Ps[2]; KPs[3] = K * Ps[3]; var KP0 = K * new Matrix(new double [, ] { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 } }); for (int i = 0; i < 4; i++) { Matrix <float> output_hom = new Matrix <float>(4, punten1px.Size); VectorOfPoint3D32F output_3d = new VectorOfPoint3D32F(); CVI.TriangulatePoints(KP0, KPs[i], punten1px, punten2px, output_hom); CVI.ConvertPointsFromHomogeneous(output_hom, output_3d); } Matrix S = U * diag * W * U.Transpose(); Matrix R = U * W * decomp.Vt; } }