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])));
}
public static SvdRes JacobiSVD(GMatrix mat)
{
//m rows, n cols
//orig mxn, U mxm, W mxn V nxn
//W is array of size n, Vt size nxn
var m = mat.rows;
var n = mat.cols;
var A = mat.tranpose().ToArray();
var res = SVD(A, m, n);
return(new SvdRes
{
U = new GMatrix(A, n, m).tranpose(), //or nxm or mxn?
W = res.W,
Vt = new GMatrix(res.Vt, n, n),
});
}
