static void Main(string[] args) { double a = 2, b = 1; double[][] initPoints = { new double[] { -a, -b, 0 }, new double[] { a, -b, 0 }, new double[] { -a, b, 0 }, new double[] { a, b, 0 } }; double[] c = { 0, 0, -1 }; double yaw = 290, pitch = -30, roll = 40; CameraParameters param = new CameraParameters(c, yaw, pitch, roll); PointD[] Bpoints = Transform.Straight(initPoints, param); CameraParameters restoredParam = Transform.Inversed(Bpoints, a, b); // compare Console.WriteLine(); ConsoleUtils.WriteLine(param.C); ConsoleUtils.WriteLine(restoredParam.C); Console.WriteLine(); ConsoleUtils.WriteLine(param.R); ConsoleUtils.WriteLine(restoredParam.R); Console.WriteLine(); ConsoleUtils.WriteLine(param.Angles); ConsoleUtils.WriteLine(restoredParam.Angles); Console.WriteLine(); ConsoleUtils.WriteLine(param.Distances(restoredParam)); Console.WriteLine(); }
public static CameraParameters Inversed_V1(PointD[] Bpoints, double a, double b) { PointD[] Bmirrored = new PointD[Bpoints.Length]; for (int i = 0; i < Bpoints.Length; i++) { Bmirrored[i] = new PointD(-Bpoints[i].X, Bpoints[i].Y); } Bpoints = Bmirrored; Line diag1 = new Line(Bpoints[0], Bpoints[3]); Line diag2 = new Line(Bpoints[1], Bpoints[2]); Line sideHB = new Line(Bpoints[0], Bpoints[1]); Line sideHT = new Line(Bpoints[2], Bpoints[3]); Line sideVL = new Line(Bpoints[0], Bpoints[2]); Line sideVR = new Line(Bpoints[1], Bpoints[3]); PointD center = diag1.Intersect(diag2); PointD perspectiveH = sideHB.Intersect(sideHT); PointD perspectiveV = sideVL.Intersect(sideVR); Line middleH = new Line(perspectiveH, center); Line middleV = new Line(perspectiveV, center); PointD[] points = new PointD[9]; for (int i = 0; i < 4; i++) { points[i] = Bpoints[i]; } points[4] = center; points[5] = middleH.Intersect(sideVL); points[6] = middleH.Intersect(sideVR); points[7] = middleV.Intersect(sideHB); points[8] = middleV.Intersect(sideHT); PointD[] origPoints = { new PointD(-a, -b), new PointD(a, -b), new PointD(-a, b), new PointD(a, b), new PointD(0, 0), new PointD(-a, 0), new PointD(a, 0), new PointD(0, -b), new PointD(0, b), }; // r11, r12, r21, r22, r31, r32, Cxr11, Cyr12, Czr13, ..., Czr33 double[,] system = new double[15, 15]; for (int i = 0; i < 9; i++) { int Xrow = 2 * i; if (Xrow >= 15) { break; } system[Xrow, 0] = origPoints[i].X; system[Xrow, 1] = origPoints[i].Y; system[Xrow, 4] = origPoints[i].X * points[i].X; system[Xrow, 5] = origPoints[i].Y * points[i].X; system[Xrow, 6] = 1; system[Xrow, 7] = 1; system[Xrow, 8] = 1; system[Xrow, 12] = -points[i].X; system[Xrow, 13] = -points[i].X; system[Xrow, 14] = -points[i].X; int Yrow = 2 * i + 1; if (Yrow >= 15) { break; } system[Yrow, 2] = origPoints[i].X; system[Yrow, 3] = origPoints[i].Y; system[Yrow, 4] = origPoints[i].X * points[i].Y; system[Yrow, 5] = origPoints[i].Y * points[i].Y; system[Yrow, 9] = 1; system[Yrow, 10] = 1; system[Yrow, 11] = 1; system[Yrow, 12] = -points[i].Y; system[Yrow, 13] = -points[i].Y; system[Yrow, 14] = -points[i].Y; } Matrix <double> mathSystem = Matrix <double> .Build.DenseOfArray(system); Evd <double> eigen = mathSystem.Evd(); Complex[] values = eigen.EigenValues.ToArray(); double[,] vectors = eigen.EigenVectors.ToArray(); int index = -1; for (int i = 0; i < values.Length; i++) { if (IsZero(values[i].Magnitude)) { index = i; } Console.WriteLine(values[i].ToString("f3")); } ConsoleUtils.WriteLine(vectors); if (index < 0) { Console.WriteLine("no solution"); return(null); } double[,] R = new double[3, 3]; double[] Cval = new double[9]; double[] C = new double[3]; for (int i = 0; i < 9; i++) { Cval[i] = vectors[i, index]; } for (int i = 0; i < 3; i++) { R[i, 0] = vectors[2 * i, index]; R[i, 1] = vectors[2 * i + 1, index]; } // norm R for (int j = 0; j < 2; j++) { double norm2 = 0; for (int i = 0; i < 3; i++) { norm2 += R[i, j] * R[i, j]; } double norm = 1 / Math.Sqrt(norm2); for (int i = 0; i < 3; i++) { R[i, j] *= norm; } } // R[i, 2] CompleteRotationMatrix(R); // C[j] R[i, j] for (int j = 0; j < 3; j++) { int denomIndex = 0; for (int i = 0; i < 3; i++) { if (Math.Abs(R[i, j]) > 0.05) { denomIndex = i; break; } } C[j] = Cval[denomIndex * 3 + j] / R[denomIndex, j]; } return(new CameraParameters(C, R)); }