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