public static GMatrix GetH2(PointFloat e, PointFloat imgSize)
{
var w2 = -imgSize.X / 2;
var h2 = -imgSize.Y / 2;
GMatrix T = new GMatrix(new double[, ] {
{ 1, 0, w2 },
{ 0, 1, h2 },
{ 0, 0, 1 }
});
var e1 = e.X + w2;
var e2 = e.Y + h2;
var l = Math.Sqrt((e1 * e1) + (e2 * e2));
var alph = e1 >= 0 ? 1 : -1;
GMatrix R = new GMatrix(new double[, ] {
{ alph *e1 / l, alph *e2 / l, 0 },
{ -1 * alph * e2 / l, alph * e1 / l, 0 },
{ 0, 0, 1 }
});
GMatrix G = new GMatrix(new double[, ]
{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ -1 / e1, 0, 1 }
});
return(GMatrix.Inverse3x3(T).dot(G.dot(R.dot(T))));
}
public static PointFloat FindEpipole(PointFloat[] pts, PointSide side, GMatrix f)
{
if (side == PointSide.Right)
{
f = f.tranpose();
}
GMatrix lines = new GMatrix(pts.Length, 3);
var storage = lines.storage;
for (int i = 0; i < pts.Length; i++)
{
var cur = storage[i];
var pt = pts[i];
var gm = f.dot(new GMatrix(new double[3, 1] {
{ pt.X }, { pt.Y }, { 1 }
}));
var ms = gm.storage;
for (int j = 0; j < 3; j++)
{
cur[j] = ms[j][0];
}
}
var svdA = JacobSvd.JacobiSVD(lines);
var res = svdA.Vt.storage[2];
//Console.WriteLine($"{res[0].ToString("0.00")},{res[1].ToString("0.00")},{res[2].ToString("0.0000000")}");
//Console.WriteLine($"{(res[0]/res[2]).ToString("0.00")},{(res[1]/res[2]).ToString("0.00")}");
return(new PointFloat((float)(res[0] / res[2]), (float)(res[1] / res[2])));
}
static PointFloat[] perspectiveTransform(PointFloat[] pts, GMatrix mat)
{
List <PointFloat> res = new List <PointFloat>();
foreach (var pt in pts)
{
var npt = mat.dot(pt.ToVect());
var w = npt.storage[2][0];
res.Add(new PointFloat(npt.storage[0][0] / w, npt.storage[1][0] / w));
}
return(res.ToArray());
}
public static GMatrix GetH1(PointFloat e, PointFloat imgSize, GMatrix F, GMatrix H2,
PointFloat[] leftPts, PointFloat[] rightPts)
{
var ex = new GMatrix(new double[, ]
{
{ 0, -1, e.Y },
{ 1, 0, -e.X },
{ -e.Y, e.X, 0 }
});
var e_111 = new GMatrix(new double[, ]
{
{ e.X, e.X, e.X },
{ e.Y, e.Y, e.Y },
{ 1, 1, 1 },
});
var m = ex.dot(F).add(e_111);
var h0h = H2.dot(m);
var m1 = perspectiveTransform(leftPts, h0h);
var m2 = perspectiveTransform(rightPts, H2);
var abuf = new double[leftPts.Length, 3];
for (var i = 0; i < leftPts.Length; i++)
{
var pt = leftPts[i];
abuf[i, 0] = pt.X;
abuf[i, 1] = pt.Y;
abuf[i, 2] = 1;
}
var A = new GMatrix(abuf);
var svcr = JacobSvd.JacobiSVD(A);
double[] B = new double[rightPts.Length];
for (var i = 0; i < rightPts.Length; i++)
{
B[i] = m2[i].X;
}
var x = Util.SVBkSb(svcr.U, svcr.W, svcr.Vt, B);
var Ha = new GMatrix(new double[, ]
{
{ x[0], x[1], x[2] },
{ 0, 1, 0 },
{ 0, 0, 1 }
});
var res = Ha.dot(h0h);
return(res);
}
