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