public static void dlt(Matrix x, Matrix xp) { var n = x.Rows; if (n < 4) { throw new Exception(); } Matrix A = null; for (int i = 0; i < x.Rows; i++) { var xip = xp[i, 0]; var yip = xp[i, 1]; var wip = xp[i, 2]; var xi = x.GetRow(i); var test = -wip * xi; double te = (-wip * xi)[0, 0]; var xit = xi.Transpose(); var row1 = new Matrix(new float[, ] { { 0, 0, 0 } }); var row2 = (wip * xit); row1 = row1.ConcateHorizontal((-wip * xit)).ConcateHorizontal((yip * xit)); row2 = row2.ConcateHorizontal(new Matrix(new float[, ] { { 0, 0, 0 } })).ConcateHorizontal((-xip * xit)); if (A == null) { A = row1.ConcateVertical(row2); } else { A.ConcateVertical(row1); A.ConcateVertical(row2); } } var svd = new SVD <float>(A); Matrix H = svd.V.Reshape(3, 3); H = H / H[2, 2]; }
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; } }