private void PrintMatricesInfo(List <Image <Arthmetic, double> > Es, Image <Arthmetic, double> K, List <Image <Arthmetic, double> > Rs, List <Image <Arthmetic, double> > ts)
{
StringBuilder sb = new StringBuilder();
Svd svd12 = new Svd(Es[0]);
Svd svd23 = new Svd(Es[1]);
Svd svd13 = new Svd(Es[2]);
sb.AppendLine(string.Format("E12 s1 = {0}, s2 = {1}", svd12.S[0, 0], svd12.S[1, 0]));
sb.AppendLine(string.Format("E23 s1 = {0}, s2 = {1}", svd23.S[0, 0], svd23.S[1, 0]));
sb.AppendLine(string.Format("E13 s1 = {0}, s2 = {1}", svd13.S[0, 0], svd13.S[1, 0]));
sb.AppendLine();
sb.AppendLine(string.Format("fx = {0}, fy = {1}", K[0, 0], K[1, 1]));
sb.AppendLine(string.Format("px = {0}, py = {1}", K[0, 2], K[1, 2]));
sb.AppendLine();
sb.AppendLine(string.Format("t12 = {0}, {1}, {2}", ts[0][0, 0], ts[0][1, 0], ts[0][2, 0]));
sb.AppendLine(string.Format("t23 = {0}, {1}, {2}", ts[1][0, 0], ts[1][1, 0], ts[1][2, 0]));
sb.AppendLine(string.Format("t13 = {0}, {1}, {2}", ts[2][0, 0], ts[2][1, 0], ts[2][2, 0]));
sb.AppendLine();
sb.AppendLine(string.Format("r12 = {0}, {1}, {2}", Rs[0][0, 0], Rs[0][1, 0], Rs[0][2, 0]));
sb.AppendLine(string.Format("r23 = {0}, {1}, {2}", Rs[1][0, 0], Rs[1][1, 0], Rs[1][2, 0]));
sb.AppendLine(string.Format("r13 = {0}, {1}, {2}", Rs[2][0, 0], Rs[2][1, 0], Rs[2][2, 0]));
info.Text = sb.ToString();
}
private void PrintMatricesInfo(Image <Arthmetic, double> E, Image <Arthmetic, double> K, Image <Arthmetic, double> R, Image <Arthmetic, double> t)
{
StringBuilder sb = new StringBuilder();
Svd svd = new Svd(E);
sb.AppendLine(string.Format("E s1 = {0}, s2 = {1}", svd.S[0, 0], svd.S[1, 0]));
sb.AppendLine();
sb.AppendLine(string.Format("fx = {0}, fy = {1}", K[0, 0], K[1, 1]));
sb.AppendLine(string.Format("px = {0}, py = {1}", K[0, 2], K[1, 2]));
sb.AppendLine();
sb.AppendLine(string.Format("tx = {0} ", t[0, 0]));
sb.AppendLine(string.Format("ty = {0}", t[1, 0]));
sb.AppendLine(string.Format("tz = {0},", t[2, 0]));
var r = RotationConverter.MatrixToEulerXYZ(R);
sb.AppendLine();
sb.AppendLine(string.Format("rx = {0} ", r[0, 0]));
sb.AppendLine(string.Format("ry = {0}", r[1, 0]));
sb.AppendLine(string.Format("rz = {0},", r[2, 0]));
info.Text = sb.ToString();
}
public double Cost(Image <Arthmetic, double> F, double fx, double fy, double px, double py, double w, double h)
{
Image <Arthmetic, double> E = ComputeMatrix.E(F, ComputeMatrix.K(fx, fy, px, py));
Svd svd = new Svd(E);
double s1 = svd.S[0, 0];
double s2 = svd.S[1, 0];
double errS = s2 == 0 ? 1.0 : (s1 - s2) / s2;
return(errS);
}
private void EigenRatioForKnownK_Click(object sender, RoutedEventArgs e)
{
PlotFunctionForErrors(() =>
{
var F = ComputeF();
var E = ComputeMatrix.E(F, K);
var svd = new Svd(E);
double error = svd.S[1, 0] / svd.S[0, 0];
return(error);
}, "s2 / s1");
}
public static double Cost(List <Image <Arthmetic, double> > Fs, double fx, double fy, double px, double py, double w, double h)
{
double errS = 0;
foreach (var F in Fs)
{
Image <Arthmetic, double> E = ComputeMatrix.E(F, ComputeMatrix.K(fx, fy, px, py));
Svd svd = new Svd(E);
double s1 = svd.S[0, 0];
double s2 = svd.S[1, 0];
errS += s2 == 0 ? 1 : (s1 - s2) / s2;
}
return(errS);
}
public static bool IsPureRotation(Image <Arthmetic, double> H, double t_ratio = 0.03, double t_scale = 0.02)
{
var svd = new Svd(H);
// If this is rotation all eigenvalues should be close to 1.0
double ratio = svd.S[2, 0] / svd.S[0, 0];
if (ratio < 1.0 - t_ratio)
{
return(false);
}
if (Math.Abs(svd.S[0, 0] - 1.0) > t_scale)
{
return(false);
}
return(true);
}
public static void DecomposeToRT(Image <Arthmetic, double> E,
out Image <Arthmetic, double>[] Rs,
out Image <Arthmetic, double>[] ts)
{
var svd = new Svd(E);
Image <Arthmetic, double> W = new Image <Arthmetic, double>(new double[, , ] {
{ { 0 }, { -1 }, { 0 } },
{ { 1 }, { 0 }, { 0 } },
{ { 0 }, { 0 }, { 1 } },
});
var R1 = svd.U.Multiply(W.T()).Multiply(svd.VT);
double det1 = CvInvoke.Determinant(R1);
if (det1 < 0)
{
R1 = R1.Mul(-1);
}
var R2 = svd.U.Multiply(W).Multiply(svd.VT);
double det2 = CvInvoke.Determinant(R2);
if (det2 < 0)
{
R2 = R2.Mul(-1);
}
double ss = (svd.S[0, 0] + svd.S[1, 0]) / 2;
Image <Arthmetic, double> Z = new Image <Arthmetic, double>(new double[, , ] {
{ { 0 }, { -ss }, { 0 } },
{ { ss }, { 0 }, { 0 } },
{ { 0 }, { 0 }, { 0 } },
});
var t1 = svd.U.Multiply(Z).Multiply(svd.U.T());
t1 = ComputeMatrix.CrossProductToVector(t1);
var t2 = t1.Mul(-1);
Rs = new Image <Arthmetic, double>[] { R1, R2 };
ts = new Image <Arthmetic, double>[] { t1, t2 };
}